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