Gyro Value to Motor-Need Help

[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();
	}

}