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.Compressor;
  11 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
  12 import edu.wpi.first.wpilibj.DoubleSolenoid;
  13 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
  14 import edu.wpi.first.wpilibj.Encoder;
  15 import edu.wpi.first.wpilibj.I2C;
  16 import edu.wpi.first.wpilibj.RobotDrive;
  17 import edu.wpi.first.wpilibj.command.PIDSubsystem;
  18
  19 public class DriveTrain extends PIDSubsystem {
  20   // Current Drive Mode Default Drive Mode is Manual
  21   private int DRIVE_MODE = 1;
  22   private static double pidOutput = 0;
  23
  24   private Encoder leftEncoder, rightEncoder;
  25
  26   public static Lidar lidar;
  27
  28   private CANTalon frontLeft, frontRight, rearLeft, rearRight;
  29   private RobotDrive robotDrive;
  30
  31   private GyroLib gyro;
  32   private DoubleSolenoid leftGearPiston, rightGearPiston;
  33   private Compressor compressor;
  34
  35   // Drivetrain specific constants that relate to the inches per pulse value for
  36   // the encoders
  37
  38   public DriveTrain() {
  39     super(Constants.DriveTrain.kp, Constants.DriveTrain.ki,
  40         Constants.DriveTrain.kd);
  41
  42     frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
  43     frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
  44     rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
  45     rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
  46
  47     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
  48
  49     lidar = new Lidar(I2C.Port.kMXP);
  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.MODULE_B_ID,
  68         Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE);
  69     rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_B_ID,
  70         Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE);
  71
  72     compressor = new Compressor(Constants.DriveTrain.COMPRESSOR_ID);
  73
  74     Constants.DriveTrain.inverted = false;
  75   }
  76
  77   @Override
  78   protected void initDefaultCommand() {
  79     setDefaultCommand(new JoystickDrive());
  80   }
  81
  82   // Print tne PID Output
  83   public void printOutput() {
  84     System.out.println("PIDOutput: " + pidOutput);
  85   }
  86
  87   private double getAvgEncoderDistance() {
  88     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
  89   }
  90
  91   // Whether or not the PID Controller thinks we have reached the target
  92   // setpoint
  93   public boolean reachedTarget() {
  94     if (this.onTarget()) {
  95       this.disable();
  96       return true;
  97     } else {
  98       return false;
  99     }
 100   }
 101
 102   public void stop() {
 103     drive(0, 0);
 104   }
 105
 106   public void resetEncoders() {
 107     leftEncoder.reset();
 108     rightEncoder.reset();
 109   }
 110
 111   public double getLidarDistance() {
 112     return lidar.pidGet();
 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(Constants.DriveTrain.kp,
 175           Constants.DriveTrain.ki, Constants.DriveTrain.kd);
 176     else
 177       this.getPIDController().setPID(Constants.DriveTrain.gp,
 178           Constants.DriveTrain.gd, Constants.DriveTrain.gi);
 179   }
 180
 181   public CANTalon getFrontLeft() {
 182     return frontLeft;
 183   }
 184
 185   public CANTalon getFrontRight() {
 186     return frontRight;
 187   }
 188
 189   public CANTalon getRearLeft() {
 190     return rearLeft;
 191   }
 192
 193   public CANTalon getRearRight() {
 194     return rearRight;
 195   }
 196
 197   public int getMode() {
 198     return DRIVE_MODE;
 199   }
 200
 201   /*
 202    * Method is a required method that the PID Subsystem uses to return the
 203    * calculated PID value to the driver
 204    *
 205    * @param Gives the user the output from the PID algorithm that is calculated
 206    * internally
 207    *
 208    * Body: Uses the output, does some filtering and drives the robot
 209    */
 210   @Override
 211   protected void usePIDOutput(double output) {
 212     double left = 0;
 213     double right = 0;
 214     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) {
 215       double drift = this.getLeftDistance() - this.getRightDistance();
 216       if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
 217         output = Math.signum(output) * 0.3;
 218       left = output;
 219       right = output + drift * Constants.DriveTrain.kp / 10;
 220     } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
 221       left = output;
 222       right = -output;
 223     }
 224     drive(left, right);
 225     pidOutput = output;
 226   }
 227
 228   @Override
 229   protected double returnPIDInput() {
 230     return sensorFeedback();
 231   }
 232
 233   /*
 234    * Checks the drive mode
 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    */
 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 negative input is required for the regular rotation
 253    * because RobotDrive tankdrive method drives inverted
 254    */
 255   public void drive(double left, double right) {
 256 //    robotDrive.tankDrive(-left, -right);
 257     // dunno why but inverted drive (- values is forward)
 258     if (!Constants.DriveTrain.inverted)
 259       robotDrive.tankDrive(-left, -right);
 260     else
 261       robotDrive.tankDrive(right, left);
 262   }
 263
 264   public void arcadeDrive(double y, double twist) {
 265     robotDrive.arcadeDrive(y, twist);
 266   }
 267
 268   /*
 269    * constrains the distance to within -100 and 100 since we aren't going to
 270    * drive more than 100 inches
 271    *
 272    * Configure Encoder PID
 273    *
 274    * Sets the setpoint to the PID subsystem
 275    */
 276   public void driveDistance(double dist, double maxTimeOut) {
 277     dist = MathLib.constrain(dist, -100, 100);
 278     setEncoderPID();
 279     setSetpoint(dist);
 280   }
 281
 282   /*
 283    * Sets the encoder mode Updates the PID constants sets the tolerance and sets
 284    * output/input ranges Enables the PID controllers
 285    */
 286   public void setEncoderPID() {
 287     DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
 288     this.updatePID();
 289     this.setAbsoluteTolerance(Constants.DriveTrain.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 Updates the PID constants, sets the tolerance and sets
 297    * output/input ranges Enables the PID controllers
 298    */
 299   private void setGyroPID() {
 300     DRIVE_MODE = Constants.DriveTrain.GYRO_MODE;
 301     this.updatePID();
 302     this.getPIDController().setPID(Constants.DriveTrain.gp,
 303         Constants.DriveTrain.gi, Constants.DriveTrain.gd);
 304
 305     this.setAbsoluteTolerance(Constants.DriveTrain.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 kReverse or
 336    * 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 kReverse or
 344    * 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
 372   public void startCompressor() {
 373     compressor.start();
 374   }
 375
 376   public void stopCompressor() {
 377     compressor.stop();
 378   }
 379
 380   public void toggleCompressor() {
 381     if (compressor.enabled())
 382       compressor.stop();
 383     else
 384       compressor.start();
 385   }
 386
 387 }