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