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 drive(double left, double right) {
 197     // Handle flipping of the "front" of the robot
 198     double k = (isFlipped() ? -1 : 1);
 199
 200     // During teleop, leftY is throttle, rightX is twist.
 201     robotDrive.arcadeDrive(-left * k, -right * k);
 202   }
 203
 204   public void setMotorSpeeds(double left, double right) {
 205     double k = (isFlipped() ? -1 : 1);
 206     robotDrive.tankDrive(-left * k, -right * k);
 207   }
 208
 209   /**
 210    * @return a value that is the current setpoint for the piston (kReverse or
 211    *         kForward)
 212    */
 213   public Value getGearPistonValue() {
 214     return leftGearPiston.get(); // Pistons should always be in the same state
 215   }
 216
 217   /**
 218    * Changes the ball shift gear assembly to high
 219    */
 220   public void setHighGear() {
 221     changeGear(Constants.DriveTrain.HIGH_GEAR);
 222   }
 223
 224   /**
 225    * Changes the ball shift gear assembly to low
 226    */
 227   public void setLowGear() {
 228     changeGear(Constants.DriveTrain.LOW_GEAR);
 229   }
 230
 231   /**
 232    * Changes the gear to a DoubleSolenoid.Value
 233    */
 234   public void changeGear(DoubleSolenoid.Value gear) {
 235     leftGearPiston.set(gear);
 236     rightGearPiston.set(gear);
 237   }
 238
 239   /**
 240    * Switches drivetrain gears from high to low or low to high
 241    */
 242   public void switchGear() {
 243     Value currentValue = getGearPistonValue();
 244     Value setValue = (currentValue == Constants.DriveTrain.HIGH_GEAR) ? Constants.DriveTrain.LOW_GEAR
 245         : Constants.DriveTrain.HIGH_GEAR;
 246     changeGear(setValue);
 247   }
 248
 249   /**
 250    * Toggle whether the motor outputs are flipped, effectively switching which
 251    * side of the robot is the front.
 252    */
 253   public void toggleFlipped() {
 254     outputFlipped = !outputFlipped;
 255   }
 256
 257   public boolean isFlipped() {
 258     return outputFlipped;
 259   }
 260
 261 }