Gyro Value to Motor-Need Help

[SandySandySand]

Hello there, I am a first year programmer with my team and we are working in Java this year, for the first time.(We are using Command-Based, instead of going for the Iterative Robot API.) We have recently looked into using a gyro for this year's autonomous mode and are having trouble. We are able to get a reading of the angle, but no matter what we try we can not modify the motor speed using this newly found angle we have. I have included the code we are using. We have tried 2 different methods to fix this problem. (The first method the code that we have commented out.) Any help is appreciated. Thank you, friends.

 protected void execute() {
    	//double heading = Robot.gyro.getRawAngle();
        //double motorPower = getSafeTan(heading);
        //Robot.drivetrain.arcadeDrive(0, motorPower);
    	double correction = 0.03;
    	double a =Robot.gyro.getRawAngle()*correction;
    	Robot.drivetrain.drive(0, a);
    }

public double getSafeTan(double heading)
    {
    	double safeAngle = heading / 45.0;
    	//now safeAngle can be NaN or -56 or some crazy useless number. 
    	//We have elected to just attempt basic corrections in that case.
    	if(safeAngle > 1)
    	{
    		safeAngle = 1;
    	}
    	
    	if(safeAngle < -1)
    	{
    		safeAngle = -1;
    	}
    	
    	if(safeAngle > 1 || safeAngle < -1)
    	{
    		safeAngle = 0;
    	}
    	
    	return safeAngle;
    }

[mehnadnerd]

I don't see anything obviously wrong. Looking at your code, I don't think it will correct for NaN errors though. By outputting your values to SmartDashboard with code like

SmartDashboard.putNumber("Raw Gyro Angle",Robot.gyro.getRawAngle())

, you can see what is going on.

[mmaunu]

When you first start up, your gyro angle might be zero because that is what it defaults to typically. So if you get your gyro angle and multiply by your .03 you get 0 for motor power. Instead of multiplying the gyros angle by some number like .03, typically you want to subtract the difference between a desired angle and the gyro angle and then multiply by that .03. For example, if you wanted to turn 90 degrees, you would take .03 times (90 - gyro.getAngle()). After finding this product, you might want to check to see if it is greater than 1 and then set it equal to 1 if it is.

[pblankenbaker]

It looks to me that you are in the process of creating a command that will rotate you robot a fixed amount and then stop and that you want to set the power used for the turn based on how far away you are from your final resting place.

This is often done with a PIDController and it appears to me you are on your way to making your own custom PIDController that uses just the "P" (proportional constant that you currently have set to 0.03).

Some general comments:

• As mmaunu pointed out, you will want to compute the error (how far off you are from the final target) and use that value when determining the power to apply.
• Make certain that your command's constructor requires() your drivetrain subsystem (I'm assuming that the drivetrain variable you referenced is a subsystem). If you fail to do this, expect nothing or strange things to happen when your command is run as other commands will likely be changing the drive power settings at the same time.
• In your initialize() method, determine the final angle you expect your robot to end up at when done.
• In your execute() method, determine the output power based on how far away you are from the final target point.
• You may need to add a minimum power check (if your robot stops because the power output gets too low before reaching the target).
• Don't forget to stop your motors in your end() and interrupted() methods.

You may find that you have "bounce" or "wiggle" as this approach is computing power output based purely as a proportion of the error. At some point in time, you may want to investigate the use of the PIDController class. It takes a bit of time to wrap your head around it, but it automates much of this process and the additional components it provides can make for much smoother operation.

Here is some rough code which demonstrates the above concepts.

package org.usfirst.frc.team868.robot.commands;

import org.usfirst.frc.team868.robot.Robot;

import edu.wpi.first.wpilibj.command.Command;

public class RotateCommand extends Command {

	// Proportional constant to compute output power based on error (P of PID)
	private static final double P = 0.03;

	// What is the maximum power setting you want to limit your output to when
	// rotating
	private static final double MAX_POWER = 1.0;

	// How close we need to be to target angle to consider ourselves done
	private static final double TOLERANCE = 3.0;

	// How far to rotate
	private double relativeRotation;

	// Current target angle we are attempting to rotate to
	private double targetAngle;

	/**
	 * Construct a new instance to of the command to rotate a specific number of
	 * degrees.
	 * 
	 * @param relativeRotation
	 *            How many degrees to rotate (pass positive values for clockwise
	 *            rotation and negative for counter clockwise).
	 */
	public RotateCommand(double relativeRotation) {
		this.relativeRotation = relativeRotation;
		targetAngle = 0;

		// IMPORTANT! Do not leave this out! This command's constructor
		// MUST require the drive subsystem in order to control it. If
		// you skip this step, expect your robot to jitter or do nothing
		// when this command is run as other commands will also likely
		// be sending output to the drive motors
		requires(Robot.drivetrain);
	}

	@Override
	protected void initialize() {
		// Compute the angle we want to end up at when we are done
		targetAngle = Robot.gyro.getAngle() + relativeRotation;
	}

	@Override
	protected void execute() {
		// Determine how far off we are from where we want to be
		double error = computeError();

		// Compute power output as a proportion of the error (as
		// we get close to desired angle, power goes to 0)
		double power = error * P;

		// Make sure power applied is within range of [-MAX_POWER, +MAX_POWER]
		power = Math.max(-MAX_POWER, Math.min(+MAX_POWER, power));

		// NOTE: You may find that you need a MIN_POWER value as well (if
		// you robot stops before making all the way to the target angle)

		// Invoke subsystem command to apply computed rotation power
		Robot.drivetrain.drive(0, power);
	}

	/**
	 * Compute how far away we are from where we want to be.
	 * 
	 * @return Negative degrees if we need to rotate counter clockwise and
	 *         positive values if we need to rotate clockwise.
	 */
	private double computeError() {
		double error = targetAngle - Robot.gyro.getAngle();
		return error;
	}

	@Override
	protected boolean isFinished() {
		// Done once we have rotated enough that our error is in range
		double error = computeError();
		boolean done = (Math.abs(error) <= TOLERANCE);
		return done;
	}

	@Override
	protected void end() {
		// Turn off motors when we are done
		Robot.drivetrain.drive(0, 0);
	}

	@Override
	protected void interrupted() {
		// Do normal clean up (turn off motors)
		end();
	}

}