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