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.FRONT_LEFT);
  33     frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
  34     rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
  35     rearRight = new CANTalon(Constants.DriveTrain.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_MODULE,
  52         Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE);
  53     rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_MODULE,
  54         Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE);
  55   }
  56
  57   @Override
  58   protected void initDefaultCommand() {
  59     setDefaultCommand(new JoystickDrive());
  60   }
  61
  62   // Print tne PID Output
  63   public void printOutput() {
  64     System.out.println("PIDOutput: " + pidOutput);
  65   }
  66
  67   private double getAvgEncoderDistance() {
  68     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
  69   }
  70
  71   // Whether or not the PID Controller thinks we have reached the target
  72   // setpoint
  73   public boolean reachedTarget() {
  74     if (this.onTarget()) {
  75       this.disable();
  76       return true;
  77     } else {
  78       return false;
  79     }
  80   }
  81
  82   public void stop() {
  83     drive(0, 0);
  84   }
  85
  86   public void resetEncoders() {
  87     leftEncoder.reset();
  88     rightEncoder.reset();
  89   }
  90
  91   public double getRightSpeed() {
  92     return rightEncoder.getRate(); // in inches per second
  93   }
  94
  95   public double getLeftSpeed() {
  96     return leftEncoder.getRate(); // in inches per second
  97   }
  98
  99   public double getSpeed() {
 100     return (getLeftSpeed() + getRightSpeed()) / 2.0; // in inches per second
 101   }
 102
 103   public double getRightDistance() {
 104     return rightEncoder.getDistance(); // in inches
 105   }
 106
 107   public double getLeftDistance() {
 108     return leftEncoder.getDistance(); // in inches
 109   }
 110
 111   // Get error between the setpoint of PID Controller and the current state of
 112   // the robot
 113   public double getError() {
 114     return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
 115   }
 116
 117   public void printEncoder(int i, int n) {
 118     if (i % n == 0) {
 119       System.out.println("Left: " + this.getLeftDistance());
 120       System.out.println("Right: " + this.getRightDistance());
 121
 122     }
 123   }
 124
 125   /*
 126    * returns the PID output that is returned by the PID Controller
 127    */
 128   public double getOutput() {
 129     return pidOutput;
 130   }
 131
 132   // Updates the PID constants based on which control mode is being used
 133   public void updatePID() {
 134     this.getPIDController().setPID(Constants.DriveTrain.kp,
 135         Constants.DriveTrain.ki, Constants.DriveTrain.kd);
 136   }
 137
 138   public CANTalon getFrontLeft() {
 139     return frontLeft;
 140   }
 141
 142   public CANTalon getFrontRight() {
 143     return frontRight;
 144   }
 145
 146   public CANTalon getRearLeft() {
 147     return rearLeft;
 148   }
 149
 150   public CANTalon getRearRight() {
 151     return rearRight;
 152   }
 153
 154   /*
 155    * Method is a required method that the PID Subsystem uses to return the
 156    * calculated PID value to the driver
 157    *
 158    * @param Gives the user the output from the PID algorithm that is calculated
 159    * internally
 160    *
 161    * Body: Uses the output, does some filtering and drives the robot
 162    */
 163   @Override
 164   protected void usePIDOutput(double output) {
 165     double left = 0;
 166     double right = 0;
 167     double drift = this.getLeftDistance() - this.getRightDistance();
 168     if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
 169       output = Math.signum(output) * 0.3;
 170     left = output;
 171     right = output + drift * Constants.DriveTrain.kp / 10;
 172     drive(left, right);
 173     pidOutput = output;
 174   }
 175
 176   @Override
 177   protected double returnPIDInput() {
 178     return sensorFeedback();
 179   }
 180
 181   /*
 182    * Checks the drive mode
 183    *
 184    * @return the current state of the robot in each state Average distance from
 185    * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
 186    */
 187   private double sensorFeedback() {
 188     return getAvgEncoderDistance();
 189   }
 190
 191   /*
 192    * @param left and right setpoints to set to the left and right side of tank
 193    * inverted is for Logan, wants the robot to invert all controls left = right
 194    * and right = left negative input is required for the regular rotation
 195    * because RobotDrive tankdrive method drives inverted
 196    */
 197   public void drive(double left, double right) {
 198     robotDrive.tankDrive(-left, -right);
 199   }
 200
 201   public void setMotorSpeeds(double left, double right) {
 202     // positive setpoint to left side makes it go backwards
 203     // positive setpoint to right side makes it go forwards.
 204     frontLeft.set(-left);
 205     rearLeft.set(-left);
 206     frontRight.set(right);
 207     rearRight.set(right);
 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 }