X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fsubsystems%2FDriveTrain.java;h=bbca72fc510cb9734b7a96fbcc5bb0face336742;hb=96215d97340b055f4f6f2d10b129552b15bbc8f8;hp=5e9f2bd93daaea7817f8b2951580351b1fa80a5c;hpb=d9c04720cc5bfc1cafab0c73e71dd231bbc65ff7;p=3501%2Fstronghold-2016 diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 5e9f2bd9..bbca72fc 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -2,25 +2,38 @@ package org.usfirst.frc.team3501.robot.subsystems; import org.usfirst.frc.team3501.robot.Constants; import org.usfirst.frc.team3501.robot.GyroLib; +import org.usfirst.frc.team3501.robot.Lidar; import org.usfirst.frc.team3501.robot.MathLib; import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive; import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.CounterBase.EncodingType; import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.I2C; 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; 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; + + public static Lidar leftLidar; + public static Lidar rightLidar; + private CANTalon frontLeft, frontRight, rearLeft, rearRight; private RobotDrive robotDrive; @@ -50,6 +63,10 @@ public class DriveTrain extends PIDSubsystem { rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT); robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight); + + leftLidar = new Lidar(I2C.Port.kOnboard); + rightLidar = new Lidar(I2C.Port.kOnboard); // TODO: find port for second + // lidar leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A, Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X); rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A, @@ -68,9 +85,10 @@ 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; } @Override @@ -78,6 +96,7 @@ public class DriveTrain extends PIDSubsystem { setDefaultCommand(new JoystickDrive()); } + // Print tne PID Output public void printOutput() { System.out.println("PIDOutput: " + pidOutput); } @@ -86,6 +105,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(); @@ -104,6 +125,14 @@ public class DriveTrain extends PIDSubsystem { rightEncoder.reset(); } + public double getLeftLidarDistance() { + return leftLidar.pidGet(); + } + + public double getsRightLidarDistance() { + return rightLidar.pidGet(); + } + public double getRightSpeed() { return rightEncoder.getRate(); // in inches per second } @@ -124,6 +153,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()); @@ -151,10 +182,14 @@ 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); @@ -182,6 +217,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; @@ -192,8 +236,7 @@ public class DriveTrain extends PIDSubsystem { output = Math.signum(output) * 0.3; left = output; right = output + drift * kp / 10; - } - else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) { + } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) { left = output; right = -output; } @@ -206,6 +249,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(); @@ -216,17 +266,42 @@ 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) { robotDrive.tankDrive(-left, -right); // dunno why but inverted drive (- values is forward) + if (!Constants.DriveTrain.inverted) + robotDrive.tankDrive(-left, + -right); + else + 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(); @@ -236,6 +311,11 @@ 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(); @@ -247,6 +327,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(); @@ -262,6 +350,41 @@ public class DriveTrain extends PIDSubsystem { rearRight.set(right); } - private static double kp = 0.013, ki = 0.000015, kd = -0.002; - private static double gp = 0.018, gi = 0.000015, gd = 0; + /* + * @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); + } }