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