Turning Robot with PID Feedback from Gyro

[ArzaanK]

Hello Chief Delphi.

For our planned off season events, our team would like to be able to use a gyroscope to turn our robot via a PID loop. We have got the gyro to work, but we cannot get the PID loop to work. The robot continuously turns, not stopping at the desired setpoint.

We confirmed the gyro works to our standard by making a command that prints the robots heading to the SmartDashboard.

The code that we are using for the PID Loop was mostly auto generated by Robot builder with a few exceptions, which are:

-The "setAbsoluteTolerance" is given a value of 3.0,
-The "returnPIDInput" function returns the getAngle() method instead of the
pidGet() method. (I'm not exactly sure what the pidGet() method does).
-The "usePIDOutput" function controls all six of our drive motors.

Please see the code posted below:

DriveTrain.java (The PID subsystem file).

package org.usfirst.frc1325.DrivetrainWithPIDControl.subsystems;
import org.usfirst.frc1325.DrivetrainWithPIDControl.RobotMap;
import org.usfirst.frc1325.DrivetrainWithPIDControl.commands.*;
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
/**
 *
 */
public class DriveTrain extends PIDSubsystem {
    
    SpeedController frontRight = RobotMap.driveTrainFrontRight;
    SpeedController middleRight = RobotMap.driveTrainMiddleRight;
    SpeedController rearRight = RobotMap.driveTrainRearRight;
    SpeedController frontLeft = RobotMap.driveTrainFrontLeft;
    SpeedController middleLeft = RobotMap.driveTrainMiddleLeft;
    SpeedController rearLeft = RobotMap.driveTrainRearLeft;
    Gyro gyro = RobotMap.driveTrainGyro;
    
    public DriveTrain() {
       
        super("DriveTrain", 1.0, 0.0, 0.0);
        setAbsoluteTolerance(3.0);
        getPIDController().setContinuous(false);
        LiveWindow.addActuator("Drive Train", "PIDSubsystem Controller", getPIDController());
    
    }
    
    public void initDefaultCommand() {
    
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    public double getGyroAngle(){
        return gyro.getAngle();
    }
    public void resetGyro(){
        gyro.reset();
    }
    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage
        return gyro.getAngle();
    }
    
    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
        frontRight.pidWrite(output);
        middleRight.pidWrite(output);
        rearRight.pidWrite(output);
        frontLeft.pidWrite(output);
        middleLeft.pidWrite(output);
        rearLeft.pidWrite(output);
    }
}

TurnRobot.java (The setpoint command)

package org.usfirst.frc1325.DrivetrainWithPIDControl.commands;

import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc1325.DrivetrainWithPIDControl.Robot;

/**
 *
 */
public class  TurnRobot extends Command {

    public TurnRobot() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
        requires(Robot.driveTrain);
        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INITIALIZE
        Robot.driveTrain.enable();
        Robot.driveTrain.setSetpoint(-90.0);
        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=INITIALIZE
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=ISFINISHED
        return Robot.driveTrain.onTarget();
        // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=ISFINISHED
    }

    // Called once after isFinished returns true
    protected void end() {
        Robot.driveTrain.disable();
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
        end();
        
    }
}

If any of you see any problems, or have any suggestions to get our PID loop working, we would greatly appreciate it.

Thanks,

[notmattlythgoe]

You posted the subsystem in both sets of code instead of the command in the second one.

[ArzaanK]

I fixed the problem. Thanks notmattlythgoe.

[notmattlythgoe]

Have you tried printing the angle from the returnPIDInput() method?

Could you be turning the opposite direction that you want to be? If you turn right and that causes the angle to count up you'll never get to -90. You'll probably also want to set your setpoint to something like

Robot.driveTrain.setSetpoint(-90.0 + Robot.driveTrain.getGyroAngle());

That way it'll always turn 90 degrees from your currently angle.

[Thad House]

It's been a while since I've done a PID loop for a drivetrain, but I think with a P of 1.0 your drive will always be turning at full power. That is because the drive accepts an input from -1 to 1, and the PID loop starts by multiplying the error by the P, which will make the output be greater then 1 whenever you are more then 1 degree off. Also, you do need to limit the PID output to be between -1 and 1 because thats the range motor outputs want.

Most likely the robot is turning the wrong way compared to the Gyro, just like notmattlythgoe said. To fix this you could return the negative gyro output, or reverse the PID output.

[krieck]

Inside the DriveTrain class, you should probably set "continuous" to "true". This indicates that when the PID input goes below its minimum, it rolls over to the maximum value.

[ArzaanK]

Have you tried printing the angle from the returnPIDInput() method?
Could you be turning the opposite direction that you want to be? If you turn right and that causes the angle to count up you'll never get to -90.

Our robot turns left when the PID Loop is enabled giving us negative angles. (This is why the gyro was set to -90.0 instead of 90.0). Also, after I disabled the PID command, I checked the reading on the gyro (I have a button on the dashboard that prints out the Gyro Angle), it was somewhere around -1000 degrees. However, I will try printing the angle from the returnPIDInput() method to see what angles I get.

a P of 1.0 your drive will always be turning at full power.

I will take that into account. So if I want the robot to turn at half speed I set P to 0.5, correct?

[Jared Russell]

Originally Posted by ArzaanK

I will take that into account. So if I want the robot to turn at half speed I set P to 0.5, correct?

PID gains have real world units attached to them. They aren't just made up numbers.

Consider a simple P controller. It has the form:

output = Kp * error

What are the units of Kp? Rearrange the equation.

Kp = output / error

So Kp is a ratio between your output (in this case, the "turn" power from -1 to 1) and your error (in this case, degrees).

Commanding an output of -1 or +1 is like commanding a full speed turn with your joysticks during teleop. For most robots, that's probably pretty fast. Probably way, way too fast to accurately turn only 1 degree.

I would start with a really tiny gain and then move up from there. Try (1/90) or so. This would give full power when you are 90 degrees (or more) off of your goal, half power at 45 degrees off, etc.

What you will likely see is that your robot moves in the general direction of your goal, but stops a little short or overshoots a bit. At this point, you can try upping your P gain, or adding Ki or Kd gains (and by the way, the same principle applies: These gains also have units, and their units involve time as well). You may also find that limiting the maximum output of your PID controller is necessary to prevent overshoots (for example, no matter what, never go more than 50% of top speed).