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,