challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java

   1 package org.usfirst.frc.team3501.robot.subsystems;
   2
   3 import org.usfirst.frc.team3501.robot.Constants;
   4 import org.usfirst.frc.team3501.robot.GyroLib;
   5 import org.usfirst.frc.team3501.robot.MathLib;
   6 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
   7
   8 import edu.wpi.first.wpilibj.CANTalon;
   9 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
  10 import edu.wpi.first.wpilibj.DoubleSolenoid;
  11 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
  12 import edu.wpi.first.wpilibj.Encoder;
  13 import edu.wpi.first.wpilibj.I2C;
  14 import edu.wpi.first.wpilibj.RobotDrive;
  15 import edu.wpi.first.wpilibj.command.PIDSubsystem;
  16
  17 public class DriveTrain extends PIDSubsystem {
  18   /*
  19    * A setpoint is the value we want the PID controller to attempt to adjust the
  20    * system to
  21    * In other words, If we want to drive the robot 4 meters, the setpoint would
  22    * be 4 meters
  23    */
  24
  25   // Encoder PID Proportional Constants P, I, and D
  26   private static double EP = 0.013, EI = 0.000015, ED = -0.002;
  27
  28   // Gyro PID Constants P, I, and D
  29   private static double GP = 0.018, GI = 0.000015, GD = 0;
  30   private static double pidOutput = 0;
  31
  32   // PID Controller tolerances for the error
  33   private static double encoderTolerance = 8.0, gyroTolerance = 5.0;
  34
  35   // Current Drive Mode Default Drive Mode is Manual
  36   private int DRIVE_MODE = 1;
  37
  38   // Different Drive Modes
  39   private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
  40
  41   private Encoder leftEncoder, rightEncoder;
  42   private CANTalon frontLeft, frontRight, rearLeft, rearRight;
  43   private RobotDrive robotDrive;
  44
  45   private GyroLib gyro;
  46   private DoubleSolenoid leftGearPiston, rightGearPiston;
  47
  48   public DriveTrain() {
  49     super(EP, EI, ED);
  50
  51     frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
  52     frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
  53     rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
  54     rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
  55
  56     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
  57     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
  58         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
  59     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
  60         Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
  61     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  62     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  63
  64     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  65     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  66
  67     gyro = new GyroLib(I2C.Port.kOnboard, false);
  68
  69     DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
  70     setEncoderPID();
  71     this.disable();
  72     gyro.start();
  73
  74     leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_FORWARD,
  75         Constants.DriveTrain.LEFT_REVERSE);
  76     rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD,
  77         Constants.DriveTrain.RIGHT_REVERSE);
  78     Constants.DriveTrain.inverted = false;
  79   }
  80
  81   @Override
  82   protected void initDefaultCommand() {
  83     setDefaultCommand(new JoystickDrive());
  84   }
  85
  86   // Print tne PID Output
  87   public void printOutput() {
  88     System.out.println("PIDOutput: " + pidOutput);
  89   }
  90
  91   private double getAvgEncoderDistance() {
  92     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
  93   }
  94
  95   // Whether or not the PID Controller thinks we have reached the target
  96   // setpoint
  97   public boolean reachedTarget() {
  98     if (this.onTarget()) {
  99       this.disable();
 100       return true;
 101     } else {
 102       return false;
 103     }
 104   }
 105
 106   public void stop() {
 107     drive(0, 0);
 108   }
 109
 110   public void resetEncoders() {
 111     leftEncoder.reset();
 112     rightEncoder.reset();
 113   }
 114
 115   public double getRightSpeed() {
 116     return rightEncoder.getRate(); // in inches per second
 117   }
 118
 119   public double getLeftSpeed() {
 120     return leftEncoder.getRate(); // in inches per second
 121   }
 122
 123   public double getSpeed() {
 124     return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
 125   }
 126
 127   public double getRightDistance() {
 128     return rightEncoder.getDistance(); // in inches
 129   }
 130
 131   public double getLeftDistance() {
 132     return leftEncoder.getDistance(); // in inches
 133   }
 134
 135   // Get error between the setpoint of PID Controller and the current state of
 136   // the robot
 137   public double getError() {
 138     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
 139       return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
 140     else
 141       return Math.abs(this.getSetpoint() + getGyroAngle());
 142   }
 143
 144   public double getGyroAngle() {
 145     return gyro.getRotationZ().getAngle();
 146   }
 147
 148   public void resetGyro() {
 149     gyro.reset();
 150   }
 151
 152   public void printEncoder(int i, int n) {
 153     if (i % n == 0) {
 154       System.out.println("Left: " + this.getLeftDistance());
 155       System.out.println("Right: " + this.getRightDistance());
 156
 157     }
 158   }
 159
 160   public void printGyroOutput() {
 161     System.out.println("Gyro Angle" + -this.getGyroAngle());
 162   }
 163
 164   /*
 165    * returns the PID output that is returned by the PID Controller
 166    */
 167   public double getOutput() {
 168     return pidOutput;
 169   }
 170
 171   // Updates the PID constants based on which control mode is being used
 172   public void updatePID() {
 173     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
 174       this.getPIDController().setPID(EP, EI, ED);
 175     else
 176       this.getPIDController().setPID(GP, GD, GI);
 177   }
 178
 179   public CANTalon getFrontLeft() {
 180     return frontLeft;
 181   }
 182
 183   public CANTalon getFrontRight() {
 184     return frontRight;
 185   }
 186
 187   public CANTalon getRearLeft() {
 188     return rearLeft;
 189   }
 190
 191   public CANTalon getRearRight() {
 192     return rearRight;
 193   }
 194
 195   public int getMode() {
 196     return DRIVE_MODE;
 197   }
 198
 199   /*
 200    * Method is a required method that the PID Subsystem uses to return the
 201    * calculated PID value to the driver
 202    *
 203    * @param Gives the user the output from the PID algorithm that is calculated
 204    * internally
 205    *
 206    * Body: Uses the output, does some filtering and drives the robot
 207    */
 208   @Override
 209   protected void usePIDOutput(double output) {
 210     double left = 0;
 211     double right = 0;
 212     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) {
 213       double drift = this.getLeftDistance() - this.getRightDistance();
 214       if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
 215         output = Math.signum(output) * 0.3;
 216       left = output;
 217       right = output + drift * EP / 10;
 218     }
 219     else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
 220       left = output;
 221       right = -output;
 222     }
 223     drive(left, right);
 224     pidOutput = output;
 225   }
 226
 227   @Override
 228   protected double returnPIDInput() {
 229     return sensorFeedback();
 230   }
 231
 232   /*
 233    * Checks the drive mode
 234    *
 235    * @return the current state of the robot in each state
 236    * Average distance from both sides of tank drive for Encoder Mode
 237    * Angle from the gyro in GYRO_MODE
 238    */
 239   private double sensorFeedback() {
 240     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
 241       return getAvgEncoderDistance();
 242     else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE)
 243       return -this.getGyroAngle();
 244     // counterclockwise is positive on joystick but we want it to be negative
 245     else
 246       return 0;
 247   }
 248
 249   /*
 250    * @param left and right setpoints to set to the left and right side of tank
 251    * inverted is for Logan, wants the robot to invert all controls left = right
 252    * and right = left
 253    * negative input is required for the regular rotation because RobotDrive
 254    * tankdrive method drives inverted
 255    */
 256   public void drive(double left, double right) {
 257     // dunno why but inverted drive (- values is forward)
 258     if (!Constants.DriveTrain.inverted)
 259       robotDrive.tankDrive(-left,
 260           -right);
 261     else
 262       robotDrive.tankDrive(right, left);
 263   }
 264
 265   /*
 266    * constrains the distance to within -100 and 100 since we aren't going to
 267    * drive more than 100 inches
 268    *
 269    * Configure Encoder PID
 270    *
 271    * Sets the setpoint to the PID subsystem
 272    */
 273   public void driveDistance(double dist, double maxTimeOut) {
 274     dist = MathLib.constrain(dist, -100, 100);
 275     setEncoderPID();
 276     setSetpoint(dist);
 277   }
 278
 279   /*
 280    * Sets the encoder mode
 281    * Updates the PID constants sets the tolerance and sets output/input ranges
 282    * Enables the PID controllers
 283    */
 284   public void setEncoderPID() {
 285     DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
 286     this.updatePID();
 287     this.setAbsoluteTolerance(encoderTolerance);
 288     this.setOutputRange(-1.0, 1.0);
 289     this.setInputRange(-200.0, 200.0);
 290     this.enable();
 291   }
 292
 293   /*
 294    * Sets the Gyro Mode
 295    * Updates the PID constants, sets the tolerance and sets output/input ranges
 296    * Enables the PID controllers
 297    */
 298   private void setGyroPID() {
 299     DRIVE_MODE = Constants.DriveTrain.GYRO_MODE;
 300     this.updatePID();
 301     this.getPIDController().setPID(GP, GI, GD);
 302
 303     this.setAbsoluteTolerance(gyroTolerance);
 304     this.setOutputRange(-1.0, 1.0);
 305     this.setInputRange(-360.0, 360.0);
 306     this.enable();
 307   }
 308
 309   /*
 310    * Turning method that should be used repeatedly in a command
 311    *
 312    * First constrains the angle to within -360 and 360 since that is as much as
 313    * we need to turn
 314    *
 315    * Configures Gyro PID and sets the setpoint as an angle
 316    */
 317   public void turnAngle(double angle) {
 318     angle = MathLib.constrain(angle, -360, 360);
 319     setGyroPID();
 320     setSetpoint(angle);
 321   }
 322
 323   public void setMotorSpeeds(double left, double right) {
 324     // positive setpoint to left side makes it go backwards
 325     // positive setpoint to right side makes it go forwards.
 326     frontLeft.set(-left);
 327     rearLeft.set(-left);
 328     frontRight.set(right);
 329     rearRight.set(right);
 330   }
 331
 332   /*
 333    * @return a value that is the current setpoint for the piston
 334    * kReverse or kForward
 335    */
 336   public Value getLeftGearPistonValue() {
 337     return leftGearPiston.get();
 338   }
 339
 340   /*
 341    * @return a value that is the current setpoint for the piston
 342    * kReverse or kForward
 343    */
 344   public Value getRightGearPistonValue() {
 345     return rightGearPiston.get();
 346   }
 347
 348   /*
 349    * Changes the ball shift gear assembly to high
 350    */
 351   public void setHighGear() {
 352     changeGear(Constants.DriveTrain.HIGH_GEAR);
 353   }
 354
 355   /*
 356    * Changes the ball shift gear assembly to low
 357    */
 358   public void setLowGear() {
 359     changeGear(Constants.DriveTrain.LOW_GEAR);
 360   }
 361
 362   /*
 363    * changes the gear to a DoubleSolenoid.Value
 364    */
 365   public void changeGear(DoubleSolenoid.Value gear) {
 366     leftGearPiston.set(gear);
 367     rightGearPiston.set(gear);
 368   }
 369 }