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.CheesyDriveHelper;
   4 import org.usfirst.frc.team3501.robot.Constants;
   5 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
   6
   7 import edu.wpi.first.wpilibj.CANTalon;
   8 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
   9 import edu.wpi.first.wpilibj.DoubleSolenoid;
  10 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
  11 import edu.wpi.first.wpilibj.Encoder;
  12 import edu.wpi.first.wpilibj.RobotDrive;
  13 import edu.wpi.first.wpilibj.command.PIDSubsystem;
  14
  15 public class DriveTrain extends PIDSubsystem {
  16   // Determines if the "front" of the robot has been reversed
  17   private boolean outputFlipped = false;
  18
  19   private static double pidOutput = 0;
  20
  21   private Encoder leftEncoder, rightEncoder;
  22
  23   private CANTalon frontLeft, frontRight, rearLeft, rearRight;
  24   private RobotDrive robotDrive;
  25
  26   private DoubleSolenoid leftGearPiston, rightGearPiston;
  27
  28   private CheesyDriveHelper cheese;
  29
  30   // Drivetrain specific constants that relate to the inches per pulse value for
  31   // the encoders
  32
  33   public DriveTrain() {
  34     super(Constants.DriveTrain.kp, Constants.DriveTrain.ki,
  35         Constants.DriveTrain.kd);
  36
  37     frontLeft = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_LEFT);
  38     frontRight = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_RIGHT);
  39     rearLeft = new CANTalon(Constants.DriveTrain.DRIVE_REAR_LEFT);
  40     rearRight = new CANTalon(Constants.DriveTrain.DRIVE_REAR_RIGHT);
  41
  42     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
  43
  44     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
  45         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
  46     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
  47         Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
  48     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  49     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  50
  51     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  52     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  53
  54     this.disable();
  55
  56     leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_SHIFT_MODULE,
  57         Constants.DriveTrain.LEFT_SHIFT_FORWARD,
  58         Constants.DriveTrain.LEFT_SHIFT_REVERSE);
  59     rightGearPiston = new DoubleSolenoid(
  60         Constants.DriveTrain.RIGHT_SHIFT_MODULE,
  61         Constants.DriveTrain.RIGHT_SHIFT_FORWARD,
  62         Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
  63
  64     cheese = new CheesyDriveHelper(this);
  65   }
  66
  67   @Override
  68   protected void initDefaultCommand() {
  69     setDefaultCommand(new JoystickDrive());
  70   }
  71
  72   // Print tne PID Output
  73   public void printOutput() {
  74     System.out.println("PIDOutput: " + pidOutput);
  75   }
  76
  77   private double getAvgEncoderDistance() {
  78     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
  79   }
  80
  81   // Whether or not the PID Controller thinks we have reached the target
  82   // setpoint
  83   public boolean reachedTarget() {
  84     if (this.onTarget()) {
  85       this.disable();
  86       return true;
  87     } else {
  88       return false;
  89     }
  90   }
  91
  92   public void stop() {
  93     setMotorSpeeds(0, 0);
  94   }
  95
  96   public void resetEncoders() {
  97     leftEncoder.reset();
  98     rightEncoder.reset();
  99   }
 100
 101   public double getRightSpeed() {
 102     return rightEncoder.getRate(); // in inches per second
 103   }
 104
 105   public double getLeftSpeed() {
 106     return leftEncoder.getRate(); // in inches per second
 107   }
 108
 109   public double getSpeed() {
 110     return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
 111   }
 112
 113   public double getRightDistance() {
 114     return rightEncoder.getDistance(); // in inches
 115   }
 116
 117   public double getLeftDistance() {
 118     return leftEncoder.getDistance(); // in inches
 119   }
 120
 121   // Get error between the setpoint of PID Controller and the current state of
 122   // the robot
 123   public double getError() {
 124     return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
 125   }
 126
 127   public void printEncoder(int i, int n) {
 128     if (i % n == 0) {
 129       System.out.println("Left: " + this.getLeftDistance());
 130       System.out.println("Right: " + this.getRightDistance());
 131
 132     }
 133   }
 134
 135   /*
 136    * returns the PID output that is returned by the PID Controller
 137    */
 138   public double getOutput() {
 139     return pidOutput;
 140   }
 141
 142   // Updates the PID constants based on which control mode is being used
 143   public void updatePID() {
 144     this.getPIDController().setPID(Constants.DriveTrain.kp,
 145         Constants.DriveTrain.ki, Constants.DriveTrain.kd);
 146   }
 147
 148   public CANTalon getFrontLeft() {
 149     return frontLeft;
 150   }
 151
 152   public CANTalon getFrontRight() {
 153     return frontRight;
 154   }
 155
 156   public CANTalon getRearLeft() {
 157     return rearLeft;
 158   }
 159
 160   public CANTalon getRearRight() {
 161     return rearRight;
 162   }
 163
 164   /*
 165    * Method is a required method that the PID Subsystem uses to return the
 166    * calculated PID value to the driver
 167    *
 168    * @param Gives the user the output from the PID algorithm that is calculated
 169    * internally
 170    *
 171    * Body: Uses the output, does some filtering and drives the robot
 172    */
 173   @Override
 174   protected void usePIDOutput(double output) {
 175     double left = 0;
 176     double right = 0;
 177     double drift = this.getLeftDistance() - this.getRightDistance();
 178     if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
 179       output = Math.signum(output) * 0.3;
 180     left = output;
 181     right = output + drift * Constants.DriveTrain.kp / 10;
 182     setMotorSpeeds(left, right);
 183     pidOutput = output;
 184   }
 185
 186   @Override
 187   protected double returnPIDInput() {
 188     return sensorFeedback();
 189   }
 190
 191   /*
 192    * Checks the drive mode
 193    *
 194    * @return the current state of the robot in each state Average distance from
 195    * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
 196    */
 197   private double sensorFeedback() {
 198     return getAvgEncoderDistance();
 199   }
 200
 201   public void joystickDrive(double left, double right) {
 202     // Handle flipping of the "front" of the robot
 203     double k = (isFlipped() ? -1 : 1);
 204     cheese.cheesyDrive(-left * k, -right,
 205         getGearPistonValue() == Constants.DriveTrain.HIGH_GEAR);
 206   }
 207
 208   public void setMotorSpeeds(double left, double right) {
 209     double k = (isFlipped() ? -1 : 1);
 210     cheese.cheesyDrive(-left * k, -right,
 211         getGearPistonValue() == Constants.DriveTrain.HIGH_GEAR);
 212   }
 213
 214   /**
 215    * @return a value that is the current setpoint for the piston (kReverse or
 216    *         kForward)
 217    */
 218   public Value getGearPistonValue() {
 219     return leftGearPiston.get(); // Pistons should always be in the same state
 220   }
 221
 222   /**
 223    * Changes the ball shift gear assembly to high
 224    */
 225   public void setHighGear() {
 226     changeGear(Constants.DriveTrain.HIGH_GEAR);
 227   }
 228
 229   /**
 230    * Changes the ball shift gear assembly to low
 231    */
 232   public void setLowGear() {
 233     changeGear(Constants.DriveTrain.LOW_GEAR);
 234   }
 235
 236   /**
 237    * Changes the gear to a DoubleSolenoid.Value
 238    */
 239   public void changeGear(DoubleSolenoid.Value gear) {
 240     leftGearPiston.set(gear);
 241     rightGearPiston.set(gear);
 242   }
 243
 244   /**
 245    * Switches drivetrain gears from high to low or low to high
 246    */
 247   public void switchGear() {
 248     Value currentValue = getGearPistonValue();
 249     Value setValue = (currentValue == Constants.DriveTrain.HIGH_GEAR) ? Constants.DriveTrain.LOW_GEAR
 250         : Constants.DriveTrain.HIGH_GEAR;
 251     changeGear(setValue);
 252   }
 253
 254   /**
 255    * Toggle whether the motor outputs are flipped, effectively switching which
 256    * side of the robot is the front.
 257    */
 258   public void toggleFlipped() {
 259     outputFlipped = !outputFlipped;
 260   }
 261
 262   public boolean isFlipped() {
 263     return outputFlipped;
 264   }
 265
 266 }