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