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   private static double kp = 0.013, ki = 0.000015, kd = -0.002;
  19   private static double gp = 0.018, gi = 0.000015, gd = 0;
  20   private static double pidOutput = 0;
  21   private static double encoderTolerance = 8.0, gyroTolerance = 5.0;
  22   private int DRIVE_MODE = 1;
  23
  24   private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
  25
  26   private Encoder leftEncoder, rightEncoder;
  27   private CANTalon frontLeft, frontRight, rearLeft, rearRight;
  28   private RobotDrive robotDrive;
  29
  30   private GyroLib gyro;
  31   private DoubleSolenoid leftGearPiston, rightGearPiston;
  32   // Drivetrain specific constants that relate to the inches per pulse value for
  33   // the encoders
  34   private final static double WHEEL_DIAMETER = 6.0; // in inches
  35   private final static double PULSES_PER_ROTATION = 256; // in pulses
  36   private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
  37   private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
  38   public final static double INCHES_PER_PULSE = (((Math.PI)
  39       * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
  40       / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
  41
  42   // Drivetrain specific constants that relate to the PID controllers
  43   private final static double Kp = 1.0, Ki = 0.0,
  44       Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
  45           / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
  46
  47   public DriveTrain() {
  48     super(kp, ki, kd);
  49
  50     frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
  51     frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
  52     rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
  53     rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
  54
  55     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
  56     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
  57         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
  58     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
  59         Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
  60     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  61     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  62
  63     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  64     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  65
  66     gyro = new GyroLib(I2C.Port.kOnboard, false);
  67
  68     DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
  69     setEncoderPID();
  70     this.disable();
  71     gyro.start();
  72
  73     leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_FORWARD,
  74         +Constants.DriveTrain.LEFT_REVERSE);
  75     rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD,
  76         Constants.DriveTrain.RIGHT_REVERSE);
  77   }
  78
  79   @Override
  80   protected void initDefaultCommand() {
  81     setDefaultCommand(new JoystickDrive());
  82   }
  83
  84   public void printOutput() {
  85     System.out.println("PIDOutput: " + pidOutput);
  86   }
  87
  88   private double getAvgEncoderDistance() {
  89     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
  90   }
  91
  92   public boolean reachedTarget() {
  93     if (this.onTarget()) {
  94       this.disable();
  95       return true;
  96     } else {
  97       return false;
  98     }
  99   }
 100
 101   public void stop() {
 102     drive(0, 0);
 103   }
 104
 105   public void resetEncoders() {
 106     leftEncoder.reset();
 107     rightEncoder.reset();
 108   }
 109
 110   public double getRightSpeed() {
 111     return rightEncoder.getRate(); // in inches per second
 112   }
 113
 114   public double getLeftSpeed() {
 115     return leftEncoder.getRate(); // in inches per second
 116   }
 117
 118   public double getSpeed() {
 119     return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
 120   }
 121
 122   public double getRightDistance() {
 123     return rightEncoder.getDistance(); // in inches
 124   }
 125
 126   public double getLeftDistance() {
 127     return leftEncoder.getDistance(); // in inches
 128   }
 129
 130   public double getError() {
 131     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
 132       return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
 133     else
 134       return Math.abs(this.getSetpoint() + getGyroAngle());
 135   }
 136
 137   public double getGyroAngle() {
 138     return gyro.getRotationZ().getAngle();
 139   }
 140
 141   public void resetGyro() {
 142     gyro.reset();
 143   }
 144
 145   public void printEncoder(int i, int n) {
 146     if (i % n == 0) {
 147       System.out.println("Left: " + this.getLeftDistance());
 148       System.out.println("Right: " + this.getRightDistance());
 149
 150     }
 151   }
 152
 153   public void printGyroOutput() {
 154     System.out.println("Gyro Angle" + -this.getGyroAngle());
 155   }
 156
 157   public double getOutput() {
 158     return pidOutput;
 159   }
 160
 161   public void updatePID() {
 162     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
 163       this.getPIDController().setPID(kp, ki, kd);
 164     else
 165       this.getPIDController().setPID(gp, gd, gi);
 166   }
 167
 168   public CANTalon getFrontLeft() {
 169     return frontLeft;
 170   }
 171
 172   public CANTalon getFrontRight() {
 173     return frontRight;
 174   }
 175
 176   public CANTalon getRearLeft() {
 177     return rearLeft;
 178   }
 179
 180   public CANTalon getRearRight() {
 181     return rearRight;
 182   }
 183
 184   public int getMode() {
 185     return DRIVE_MODE;
 186   }
 187
 188   @Override
 189   protected void usePIDOutput(double output) {
 190     double left = 0;
 191     double right = 0;
 192     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) {
 193       double drift = this.getLeftDistance() - this.getRightDistance();
 194       if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
 195         output = Math.signum(output) * 0.3;
 196       left = output;
 197       right = output + drift * kp / 10;
 198     }
 199     else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
 200       left = output;
 201       right = -output;
 202     }
 203     drive(left, right);
 204     pidOutput = output;
 205   }
 206
 207   @Override
 208   protected double returnPIDInput() {
 209     return sensorFeedback();
 210   }
 211
 212   private double sensorFeedback() {
 213     if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
 214       return getAvgEncoderDistance();
 215     else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE)
 216       return -this.getGyroAngle();
 217     // counterclockwise is positive on joystick but we want it to be negative
 218     else
 219       return 0;
 220   }
 221
 222   public void drive(double left, double right) {
 223     robotDrive.tankDrive(-left, -right);
 224     // dunno why but inverted drive (- values is forward)
 225   }
 226
 227   public void driveDistance(double dist, double maxTimeOut) {
 228     dist = MathLib.constrain(dist, -100, 100);
 229     setEncoderPID();
 230     setSetpoint(dist);
 231   }
 232
 233   public void setEncoderPID() {
 234     DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
 235     this.updatePID();
 236     this.setAbsoluteTolerance(encoderTolerance);
 237     this.setOutputRange(-1.0, 1.0);
 238     this.setInputRange(-200.0, 200.0);
 239     this.enable();
 240   }
 241
 242   private void setGyroPID() {
 243     DRIVE_MODE = Constants.DriveTrain.GYRO_MODE;
 244     this.updatePID();
 245     this.getPIDController().setPID(gp, gi, gd);
 246
 247     this.setAbsoluteTolerance(gyroTolerance);
 248     this.setOutputRange(-1.0, 1.0);
 249     this.setInputRange(-360.0, 360.0);
 250     this.enable();
 251   }
 252
 253   public void turnAngle(double angle) {
 254     angle = MathLib.constrain(angle, -360, 360);
 255     setGyroPID();
 256     setSetpoint(angle);
 257   }
 258
 259   public void setMotorSpeeds(double left, double right) {
 260     // positive setpoint to left side makes it go backwards
 261     // positive setpoint to right side makes it go forwards.
 262     frontLeft.set(-left);
 263     rearLeft.set(-left);
 264     frontRight.set(right);
 265     rearRight.set(right);
 266   }
 267
 268   public Value getLeftGearPistonValue() {
 269     return leftGearPiston.get();
 270   }
 271
 272   public Value getRightGearPistonValue() {
 273     return rightGearPiston.get();
 274   }
 275
 276   public void setHighGear() {
 277     changeGear(Constants.DriveTrain.HIGH_GEAR);
 278   }
 279
 280   public void setLowGear() {
 281     changeGear(Constants.DriveTrain.LOW_GEAR);
 282   }
 283
 284   public void changeGear(DoubleSolenoid.Value gear) {
 285     leftGearPiston.set(gear);
 286     rightGearPiston.set(gear);
 287   }
 288 }