From 1c94f23033fba14b780a960e93b735ac28e6effa Mon Sep 17 00:00:00 2001 From: Kevin Zhang Date: Mon, 15 Feb 2016 14:11:05 -0800 Subject: [PATCH] Add comments to all of the driveTrain PID commands and methods to explain what is going on --- .../robot/commands/driving/DriveDistance.java | 16 ++- .../robot/commands/driving/JoystickDrive.java | 5 +- .../robot/commands/driving/TurnForAngle.java | 16 ++- .../team3501/robot/subsystems/DriveTrain.java | 113 ++++++++++++++---- 4 files changed, 111 insertions(+), 39 deletions(-) diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java index 1eb707c0..81b6c5c1 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/DriveDistance.java @@ -6,13 +6,18 @@ import org.usfirst.frc.team3501.robot.Robot; import edu.wpi.first.wpilibj.command.Command; /*** - * This command will move the robot at the specified speed for the specified - * distance. + * This command will drive the drivetrain a certain distance in inches * - * post-condition: has driven for the distance and speed specified - * - * @author Meryem and Avi + * @param distance + * is the distance we want to drive + * maxTimeOut is a catch just in case the robot malfunctions and never + * gets to the setpoint * + * @code + * Repeatedly updates the driveTrain setpoint + * Finishes when the time goes over maxTimeOut or the driveTrain hits the + * setpoint + * end() disables the PID driveTrain */ public class DriveDistance extends Command { private double maxTimeOut; @@ -55,6 +60,5 @@ public class DriveDistance extends Command { @Override protected void interrupted() { - end(); } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java index 41e44d31..5d8ff9c9 100644 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/JoystickDrive.java @@ -5,7 +5,9 @@ import org.usfirst.frc.team3501.robot.Robot; import edu.wpi.first.wpilibj.command.Command; /** - * + * Runs throughout teleop and listens for joystick inputs and drives the + * driveTrain + * Never finishes until teleop ends */ public class JoystickDrive extends Command { @@ -37,6 +39,5 @@ public class JoystickDrive extends Command { @Override protected void interrupted() { - end(); } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java index f9e5e890..2362b89b 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java +++ b/src/org/usfirst/frc/team3501/robot/commands/driving/TurnForAngle.java @@ -6,18 +6,17 @@ import org.usfirst.frc.team3501.robot.Robot; import edu.wpi.first.wpilibj.command.Command; /*** - * This command will move the robot at the specified speed for the specified - * distance. - * - * post-condition: has driven for the distance and speed specified - * - * @author Meryem and Avi - * + * @param angle + * is the setpoint we want to turn for + * maxTimeOut catch just in case robot malfunctions and never reaches + * setpoint + * @initialize resets the Gyro and prints the current mode + * @code repeatedly sets a new setpoint angle to the motor + * @end ends when the setpoint is reached. */ public class TurnForAngle extends Command { private double maxTimeOut; double angle; - int count = 0; public TurnForAngle(double angle, double maxTimeOut) { requires(Robot.driveTrain); @@ -36,7 +35,6 @@ public class TurnForAngle extends Command { Robot.driveTrain.turnAngle(angle); Robot.driveTrain.printGyroOutput(); Robot.driveTrain.printOutput(); - count++; } diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 1282be1b..c88d4c42 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -15,12 +15,20 @@ import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.command.PIDSubsystem; public class DriveTrain extends PIDSubsystem { - private static double kp = 0.013, ki = 0.000015, kd = -0.002; - private static double gp = 0.018, gi = 0.000015, gd = 0; + // Encoder PID Proportional Constants P, I, and D + private static double EP = 0.013, EI = 0.000015, ED = -0.002; + + // Gyro PID Constants P, I, and D + private static double GP = 0.018, GI = 0.000015, GD = 0; private static double pidOutput = 0; + + // PID Controller tolerances for the error private static double encoderTolerance = 8.0, gyroTolerance = 5.0; + + // Current Drive Mode Default Drive Mode is Manual private int DRIVE_MODE = 1; + // Different Drive Modes private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3; private Encoder leftEncoder, rightEncoder; @@ -29,23 +37,9 @@ public class DriveTrain extends PIDSubsystem { private GyroLib gyro; private DoubleSolenoid leftGearPiston, rightGearPiston; - // Drivetrain specific constants that relate to the inches per pulse value for - // the encoders - private final static double WHEEL_DIAMETER = 6.0; // in inches - private final static double PULSES_PER_ROTATION = 256; // in pulses - private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches - private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches - public final static double INCHES_PER_PULSE = (((Math.PI) - * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) - / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER; - - // Drivetrain specific constants that relate to the PID controllers - private final static double Kp = 1.0, Ki = 0.0, - Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) - / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER; public DriveTrain() { - super(kp, ki, kd); + super(EP, EI, ED); frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT); frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT); @@ -71,7 +65,7 @@ public class DriveTrain extends PIDSubsystem { gyro.start(); leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_FORWARD, - +Constants.DriveTrain.LEFT_REVERSE); + Constants.DriveTrain.LEFT_REVERSE); rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE); Constants.DriveTrain.inverted = false; @@ -82,6 +76,7 @@ public class DriveTrain extends PIDSubsystem { setDefaultCommand(new JoystickDrive()); } + // Print tne PID Output public void printOutput() { System.out.println("PIDOutput: " + pidOutput); } @@ -90,6 +85,8 @@ public class DriveTrain extends PIDSubsystem { return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2; } + // Whether or not the PID Controller thinks we have reached the target + // setpoint public boolean reachedTarget() { if (this.onTarget()) { this.disable(); @@ -128,6 +125,8 @@ public class DriveTrain extends PIDSubsystem { return leftEncoder.getDistance(); // in inches } + // Get error between the setpoint of PID Controller and the current state of + // the robot public double getError() { if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) return Math.abs(this.getSetpoint() - getAvgEncoderDistance()); @@ -155,15 +154,19 @@ public class DriveTrain extends PIDSubsystem { System.out.println("Gyro Angle" + -this.getGyroAngle()); } + /* + * returns the PID output that is returned by the PID Controller + */ public double getOutput() { return pidOutput; } + // Updates the PID constants based on which control mode is being used public void updatePID() { if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) - this.getPIDController().setPID(kp, ki, kd); + this.getPIDController().setPID(EP, EI, ED); else - this.getPIDController().setPID(gp, gd, gi); + this.getPIDController().setPID(GP, GD, GI); } public CANTalon getFrontLeft() { @@ -186,6 +189,15 @@ public class DriveTrain extends PIDSubsystem { return DRIVE_MODE; } + /* + * Method is a required method that the PID Subsystem uses to return the + * calculated PID value to the driver + * + * @param Gives the user the output from the PID algorithm that is calculated + * internally + * + * Body: Uses the output, does some filtering and drives the robot + */ @Override protected void usePIDOutput(double output) { double left = 0; @@ -195,7 +207,7 @@ public class DriveTrain extends PIDSubsystem { if (Math.abs(output) > 0 && Math.abs(output) < 0.3) output = Math.signum(output) * 0.3; left = output; - right = output + drift * kp / 10; + right = output + drift * EP / 10; } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) { left = output; @@ -210,6 +222,13 @@ public class DriveTrain extends PIDSubsystem { return sensorFeedback(); } + /* + * Checks the drive mode + * + * @return the current state of the robot in each state + * Average distance from both sides of tank drive for Encoder Mode + * Angle from the gyro in GYRO_MODE + */ private double sensorFeedback() { if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) return getAvgEncoderDistance(); @@ -220,6 +239,13 @@ public class DriveTrain extends PIDSubsystem { return 0; } + /* + * @param left and right setpoints to set to the left and right side of tank + * inverted is for Logan, wants the robot to invert all controls left = right + * and right = left + * negative input is required for the regular rotation because RobotDrive + * tankdrive method drives inverted + */ public void drive(double left, double right) { // dunno why but inverted drive (- values is forward) if (!Constants.DriveTrain.inverted) @@ -229,12 +255,25 @@ public class DriveTrain extends PIDSubsystem { robotDrive.tankDrive(right, left); } + /* + * constrains the distance to within -100 and 100 since we aren't going to + * drive more than 100 inches + * + * Configure Encoder PID + * + * Sets the setpoint to the PID subsystem + */ public void driveDistance(double dist, double maxTimeOut) { dist = MathLib.constrain(dist, -100, 100); setEncoderPID(); setSetpoint(dist); } + /* + * Sets the encoder mode + * Updates the PID constants sets the tolerance and sets output/input ranges + * Enables the PID controllers + */ public void setEncoderPID() { DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE; this.updatePID(); @@ -244,10 +283,15 @@ public class DriveTrain extends PIDSubsystem { this.enable(); } + /* + * Sets the Gyro Mode + * Updates the PID constants, sets the tolerance and sets output/input ranges + * Enables the PID controllers + */ private void setGyroPID() { DRIVE_MODE = Constants.DriveTrain.GYRO_MODE; this.updatePID(); - this.getPIDController().setPID(gp, gi, gd); + this.getPIDController().setPID(GP, GI, GD); this.setAbsoluteTolerance(gyroTolerance); this.setOutputRange(-1.0, 1.0); @@ -255,6 +299,14 @@ public class DriveTrain extends PIDSubsystem { this.enable(); } + /* + * Turning method that should be used repeatedly in a command + * + * First constrains the angle to within -360 and 360 since that is as much as + * we need to turn + * + * Configures Gyro PID and sets the setpoint as an angle + */ public void turnAngle(double angle) { angle = MathLib.constrain(angle, -360, 360); setGyroPID(); @@ -270,22 +322,39 @@ public class DriveTrain extends PIDSubsystem { rearRight.set(right); } + /* + * @return a value that is the current setpoint for the piston + * kReverse or kForward + */ public Value getLeftGearPistonValue() { return leftGearPiston.get(); } + /* + * @return a value that is the current setpoint for the piston + * kReverse or kForward + */ public Value getRightGearPistonValue() { return rightGearPiston.get(); } + /* + * Changes the ball shift gear assembly to high + */ public void setHighGear() { changeGear(Constants.DriveTrain.HIGH_GEAR); } + /* + * Changes the ball shift gear assembly to low + */ public void setLowGear() { changeGear(Constants.DriveTrain.LOW_GEAR); } + /* + * changes the gear to a DoubleSolenoid.Value + */ public void changeGear(DoubleSolenoid.Value gear) { leftGearPiston.set(gear); rightGearPiston.set(gear); -- 2.30.2