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   private boolean outputFlipped = false;
  16   private static double pidOutput = 0;
  17
  18   private Encoder leftEncoder, rightEncoder;
  19
  20   private CANTalon frontLeft, frontRight, rearLeft, rearRight;
  21   private RobotDrive robotDrive;
  22
  23   private DoubleSolenoid leftGearPiston, rightGearPiston;
  24
  25   // Drivetrain specific constants that relate to the inches per pulse value for
  26   // the encoders
  27
  28   public DriveTrain() {
  29     super(Constants.DriveTrain.kp, Constants.DriveTrain.ki,
  30         Constants.DriveTrain.kd);
  31
  32     frontLeft = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_LEFT);
  33     frontRight = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_RIGHT);
  34     rearLeft = new CANTalon(Constants.DriveTrain.DRIVE_REAR_LEFT);
  35     rearRight = new CANTalon(Constants.DriveTrain.DRIVE_REAR_RIGHT);
  36
  37     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
  38
  39     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
  40         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
  41     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
  42         Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
  43     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  44     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  45
  46     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  47     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
  48
  49     this.disable();
  50
  51     leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_SHIFT_MODULE,
  52         Constants.DriveTrain.LEFT_SHIFT_FORWARD,
  53         Constants.DriveTrain.LEFT_SHIFT_REVERSE);
  54     rightGearPiston = new DoubleSolenoid(
  55         Constants.DriveTrain.RIGHT_SHIFT_MODULE,
  56         Constants.DriveTrain.RIGHT_SHIFT_FORWARD,
  57         Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
  58   }
  59
  60   @Override
  61   protected void initDefaultCommand() {
  62     setDefaultCommand(new JoystickDrive());
  63   }
  64
  65   // Print tne PID Output
  66   public void printOutput() {
  67     System.out.println("PIDOutput: " + pidOutput);
  68   }
  69
  70   private double getAvgEncoderDistance() {
  71     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
  72   }
  73
  74   // Whether or not the PID Controller thinks we have reached the target
  75   // setpoint
  76   public boolean reachedTarget() {
  77     if (this.onTarget()) {
  78       this.disable();
  79       return true;
  80     } else {
  81       return false;
  82     }
  83   }
  84
  85   public void stop() {
  86     setMotorSpeeds(0, 0);
  87   }
  88
  89   public void resetEncoders() {
  90     leftEncoder.reset();
  91     rightEncoder.reset();
  92   }
  93
  94   public double getRightSpeed() {
  95     return rightEncoder.getRate(); // in inches per second
  96   }
  97
  98   public double getLeftSpeed() {
  99     return leftEncoder.getRate(); // in inches per second
 100   }
 101
 102   public double getSpeed() {
 103     return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
 104   }
 105
 106   public double getRightDistance() {
 107     return rightEncoder.getDistance(); // in inches
 108   }
 109
 110   public double getLeftDistance() {
 111     return leftEncoder.getDistance(); // in inches
 112   }
 113
 114   // Get error between the setpoint of PID Controller and the current state of
 115   // the robot
 116   public double getError() {
 117     return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
 118   }
 119
 120   public void printEncoder(int i, int n) {
 121     if (i % n == 0) {
 122       System.out.println("Left: " + this.getLeftDistance());
 123       System.out.println("Right: " + this.getRightDistance());
 124
 125     }
 126   }
 127
 128   /*
 129    * returns the PID output that is returned by the PID Controller
 130    */
 131   public double getOutput() {
 132     return pidOutput;
 133   }
 134
 135   // Updates the PID constants based on which control mode is being used
 136   public void updatePID() {
 137     this.getPIDController().setPID(Constants.DriveTrain.kp,
 138         Constants.DriveTrain.ki, Constants.DriveTrain.kd);
 139   }
 140
 141   public CANTalon getFrontLeft() {
 142     return frontLeft;
 143   }
 144
 145   public CANTalon getFrontRight() {
 146     return frontRight;
 147   }
 148
 149   public CANTalon getRearLeft() {
 150     return rearLeft;
 151   }
 152
 153   public CANTalon getRearRight() {
 154     return rearRight;
 155   }
 156
 157   /*
 158    * Method is a required method that the PID Subsystem uses to return the
 159    * calculated PID value to the driver
 160    *
 161    * @param Gives the user the output from the PID algorithm that is calculated
 162    * internally
 163    *
 164    * Body: Uses the output, does some filtering and drives the robot
 165    */
 166   @Override
 167   protected void usePIDOutput(double output) {
 168     double left = 0;
 169     double right = 0;
 170     double drift = this.getLeftDistance() - this.getRightDistance();
 171     if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
 172       output = Math.signum(output) * 0.3;
 173     left = output;
 174     right = output + drift * Constants.DriveTrain.kp / 10;
 175     setMotorSpeeds(left, right);
 176     pidOutput = output;
 177   }
 178
 179   @Override
 180   protected double returnPIDInput() {
 181     return sensorFeedback();
 182   }
 183
 184   /*
 185    * Checks the drive mode
 186    *
 187    * @return the current state of the robot in each state Average distance from
 188    * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
 189    */
 190   private double sensorFeedback() {
 191     return getAvgEncoderDistance();
 192   }
 193
 194   public void joystickDrive(double left, double right) {
 195     int type = Constants.DriveTrain.DRIVE_TYPE;
 196     double k = (isFlipped() ? -1 : 1);
 197     if (type == Constants.DriveTrain.TANK) {
 198       robotDrive.tankDrive(-left * k, -right * k);
 199     } else if (type == Constants.DriveTrain.ARCADE) {
 200       robotDrive.arcadeDrive(left * k, right);
 201     }
 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 }