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