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