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.MathLib;
   5 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
   6 import org.usfirst.frc.team3501.robot.sensors.GyroLib;
   7 import org.usfirst.frc.team3501.robot.sensors.Lidar;
   8
   9 import edu.wpi.first.wpilibj.CANTalon;
  10 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
  11 import edu.wpi.first.wpilibj.DoubleSolenoid;
  12 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
  13 import edu.wpi.first.wpilibj.Encoder;
  14 import edu.wpi.first.wpilibj.I2C;
  15 import edu.wpi.first.wpilibj.RobotDrive;
  16 import edu.wpi.first.wpilibj.command.PIDSubsystem;
  17
  18 public class DriveTrain extends PIDSubsystem {
  19   // Current Drive Mode Default Drive Mode is Manual
  20   private int DRIVE_MODE = 1;
  21   private static double pidOutput = 0;
  22
  23   private Encoder leftEncoder, rightEncoder;
  24
  25   public static Lidar lidar;
  26
  27   private CANTalon frontLeft, frontRight, rearLeft, rearRight;
  28   private RobotDrive robotDrive;
  29
  30   private GyroLib gyro;
  31   private DoubleSolenoid leftGearPiston, rightGearPiston;
  32
  33   // Drivetrain specific constants that relate to the inches per pulse value for
  34   // the encoders
  35
  36   public DriveTrain() {
  37     super(Constants.DriveTrain.kp, Constants.DriveTrain.ki,
  38         Constants.DriveTrain.kd);
  39
  40     frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
  41     frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
  42     rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
  43     rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
  44
  45     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
  46
  47     lidar = new Lidar(I2C.Port.kOnboard);
  48
  49     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
  50         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
  51     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
  52         Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
  53     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  54     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  55
  56     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  57     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  58
  59     gyro = new GyroLib(I2C.Port.kOnboard, false);
  60
  61     DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
  62     setEncoderPID();
  63     this.disable();
  64     gyro.start();
  65
  66     leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_FORWARD,
  67         Constants.DriveTrain.LEFT_REVERSE);
  68     rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD,
  69         Constants.DriveTrain.RIGHT_REVERSE);
  70     Constants.DriveTrain.inverted = false;
  71   }
  72
  73   @Override
  74   protected void initDefaultCommand() {
  75     setDefaultCommand(new JoystickDrive());
  76   }
  77
  78   // Print tne PID Output
  79   public void printOutput() {
  80     System.out.println("PIDOutput: " + pidOutput);
  81   }
  82
  83   private double getAvgEncoderDistance() {
  84     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
  85   }
  86
  87   // Whether or not the PID Controller thinks we have reached the target
  88   // setpoint
  89   public boolean reachedTarget() {
  90     if (this.onTarget()) {
  91       this.disable();
  92       return true;
  93     } else {
  94       return false;
  95     }
  96   }
  97
  98   public void stop() {
  99     drive(0, 0);
 100   }
 101
 102   public void resetEncoders() {
 103     leftEncoder.reset();
 104     rightEncoder.reset();
 105   }
 106
 107   public double getLidarDistance() {
 108     return lidar.pidGet();
 109   }
 110
 111   public double getRightSpeed() {
 112     return rightEncoder.getRate(); // in inches per second
 113   }
 114
 115   public double getLeftSpeed() {
 116     return leftEncoder.getRate(); // in inches per second
 117   }
 118
 119   public double getSpeed() {
 120     return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
 121   }
 122
 123   public double getRightDistance() {
 124     return rightEncoder.getDistance(); // in inches
 125   }
 126
 127   public double getLeftDistance() {
 128     return leftEncoder.getDistance(); // in inches
 129   }
 130
 131   // Get error between the setpoint of PID Controller and the current state of
 132   // the robot
 133   public double getError() {
 134     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
 135       return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
 136     else
 137       return Math.abs(this.getSetpoint() + getGyroAngle());
 138   }
 139
 140   public double getGyroAngle() {
 141     return gyro.getRotationZ().getAngle();
 142   }
 143
 144   public void resetGyro() {
 145     gyro.reset();
 146   }
 147
 148   public void printEncoder(int i, int n) {
 149     if (i % n == 0) {
 150       System.out.println("Left: " + this.getLeftDistance());
 151       System.out.println("Right: " + this.getRightDistance());
 152
 153     }
 154   }
 155
 156   public void printGyroOutput() {
 157     System.out.println("Gyro Angle" + -this.getGyroAngle());
 158   }
 159
 160   /*
 161    * returns the PID output that is returned by the PID Controller
 162    */
 163   public double getOutput() {
 164     return pidOutput;
 165   }
 166
 167   // Updates the PID constants based on which control mode is being used
 168   public void updatePID() {
 169     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
 170       this.getPIDController().setPID(Constants.DriveTrain.kp,
 171           Constants.DriveTrain.ki, Constants.DriveTrain.kd);
 172     else
 173       this.getPIDController().setPID(Constants.DriveTrain.gp,
 174           Constants.DriveTrain.gd, Constants.DriveTrain.gi);
 175   }
 176
 177   public CANTalon getFrontLeft() {
 178     return frontLeft;
 179   }
 180
 181   public CANTalon getFrontRight() {
 182     return frontRight;
 183   }
 184
 185   public CANTalon getRearLeft() {
 186     return rearLeft;
 187   }
 188
 189   public CANTalon getRearRight() {
 190     return rearRight;
 191   }
 192
 193   public int getMode() {
 194     return DRIVE_MODE;
 195   }
 196
 197   /*
 198    * Method is a required method that the PID Subsystem uses to return the
 199    * calculated PID value to the driver
 200    *
 201    * @param Gives the user the output from the PID algorithm that is calculated
 202    * internally
 203    *
 204    * Body: Uses the output, does some filtering and drives the robot
 205    */
 206   @Override
 207   protected void usePIDOutput(double output) {
 208     double left = 0;
 209     double right = 0;
 210     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) {
 211       double drift = this.getLeftDistance() - this.getRightDistance();
 212       if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
 213         output = Math.signum(output) * 0.3;
 214       left = output;
 215       right = output + drift * Constants.DriveTrain.kp / 10;
 216     } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
 217       left = output;
 218       right = -output;
 219     }
 220     drive(left, right);
 221     pidOutput = output;
 222   }
 223
 224   @Override
 225   protected double returnPIDInput() {
 226     return sensorFeedback();
 227   }
 228
 229   /*
 230    * Checks the drive mode <<<<<<< 9728080f491e9fb09795494349dba1297f447c0f
 231    *
 232    * @return the current state of the robot in each state Average distance from
 233    * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
 234    * =======
 235    *
 236    * @return the current state of the robot in each state Average distance from
 237    * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
 238    * >>>>>>> Move all constants in DeadReckoning to Auton class because it makes
 239    * more sense
 240    */
 241   private double sensorFeedback() {
 242     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
 243       return getAvgEncoderDistance();
 244     else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE)
 245       return -this.getGyroAngle();
 246     // counterclockwise is positive on joystick but we want it to be negative
 247     else
 248       return 0;
 249   }
 250
 251   /*
 252    * @param left and right setpoints to set to the left and right side of tank
 253    * inverted is for Logan, wants the robot to invert all controls left = right
 254    * and right = left negative input is required for the regular rotation
 255    * because RobotDrive tankdrive method drives inverted
 256    */
 257   public void drive(double left, double right) {
 258     robotDrive.tankDrive(-left, -right);
 259     // dunno why but inverted drive (- values is forward)
 260     if (!Constants.DriveTrain.inverted)
 261       robotDrive.tankDrive(-left, -right);
 262     else
 263       robotDrive.tankDrive(right, left);
 264   }
 265
 266   /*
 267    * constrains the distance to within -100 and 100 since we aren't going to
 268    * drive more than 100 inches
 269    *
 270    * Configure Encoder PID
 271    *
 272    * Sets the setpoint to the PID subsystem
 273    */
 274   public void driveDistance(double dist, double maxTimeOut) {
 275     dist = MathLib.constrain(dist, -100, 100);
 276     setEncoderPID();
 277     setSetpoint(dist);
 278   }
 279
 280   /*
 281    * Sets the encoder mode Updates the PID constants sets the tolerance and sets
 282    * output/input ranges Enables the PID controllers
 283    */
 284   public void setEncoderPID() {
 285     DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
 286     this.updatePID();
 287     this.setAbsoluteTolerance(Constants.DriveTrain.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 Updates the PID constants, sets the tolerance and sets
 295    * output/input ranges Enables the PID controllers
 296    */
 297   private void setGyroPID() {
 298     DRIVE_MODE = Constants.DriveTrain.GYRO_MODE;
 299     this.updatePID();
 300     this.getPIDController().setPID(Constants.DriveTrain.gp,
 301         Constants.DriveTrain.gi, Constants.DriveTrain.gd);
 302
 303     this.setAbsoluteTolerance(Constants.DriveTrain.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 kReverse or
 334    * 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 kReverse or
 342    * 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
 370   public void toggleTimeDeadReckoning() {
 371     Constants.Auton.isUsingTimeToPassDefense = !Constants.Auton.isUsingTimeToPassDefense;
 372   }
 373 }