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