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