X-Git-Url: http://challenge-bot.com/repos/?a=blobdiff_plain;f=src%2Forg%2Fusfirst%2Ffrc%2Fteam3501%2Frobot%2Fsubsystems%2FDriveTrain.java;h=1a4e6e5b96c04dfc58689e377aa2ec5ba9dd1ae8;hb=949415d065993cac3fd0c271eb3ba5d6b5ec90af;hp=0d1db15b930268ccea2a86b65627118080932e14;hpb=ff9a504fa2aa658265087e163120b628b6da4129;p=3501%2Fstronghold-2016

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 0d1db15b..1a4e6e5b 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -1,8 +1,6 @@
 package org.usfirst.frc.team3501.robot.subsystems;
 
 import org.usfirst.frc.team3501.robot.Constants;
-import org.usfirst.frc.team3501.robot.GyroLib;
-import org.usfirst.frc.team3501.robot.MathLib;
 import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
 
 import edu.wpi.first.wpilibj.CANTalon;
@@ -10,57 +8,36 @@ import edu.wpi.first.wpilibj.CounterBase.EncodingType;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.RobotDrive;
 import edu.wpi.first.wpilibj.command.PIDSubsystem;
 
 public class DriveTrain extends PIDSubsystem {
-  private static double kp = 0.013, ki = 0.000015, kd = -0.002;
-  private static double gp = 0.018, gi = 0.000015, gd = 0;
-  private static double pidOutput = 0;
-  private static double encoderTolerance = 8.0, gyroTolerance = 5.0;
-  private int DRIVE_MODE = 1;
+  // Determines if the "front" of the robot has been reversed
+  private boolean outputFlipped = false;
 
-  private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
+  private static double pidOutput = 0;
 
   private Encoder leftEncoder, rightEncoder;
 
-  public static Lidar leftLidar;
-  public static Lidar rightLidar;
-
   private CANTalon frontLeft, frontRight, rearLeft, rearRight;
   private RobotDrive robotDrive;
 
-  private GyroLib gyro;
   private DoubleSolenoid leftGearPiston, rightGearPiston;
+
   // Drivetrain specific constants that relate to the inches per pulse value for
   // the encoders
-  private final static double WHEEL_DIAMETER = 6.0; // in inches
-  private final static double PULSES_PER_ROTATION = 256; // in pulses
-  private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
-  private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
-  public final static double INCHES_PER_PULSE = (((Math.PI)
-      * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
-      / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
-
-  // Drivetrain specific constants that relate to the PID controllers
-  private final static double Kp = 1.0, Ki = 0.0,
-      Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
-          / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
 
   public DriveTrain() {
-    super(kp, ki, kd);
+    super(Constants.DriveTrain.kp, Constants.DriveTrain.ki,
+        Constants.DriveTrain.kd);
 
-    frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
-    frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
-    rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
-    rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
+    frontLeft = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_LEFT);
+    frontRight = new CANTalon(Constants.DriveTrain.DRIVE_FRONT_RIGHT);
+    rearLeft = new CANTalon(Constants.DriveTrain.DRIVE_REAR_LEFT);
+    rearRight = new CANTalon(Constants.DriveTrain.DRIVE_REAR_RIGHT);
 
     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
 
-    leftLidar = new Lidar(I2C.Port.kOnboard);
-    rightLidar = new Lidar(I2C.Port.kOnboard); // TODO: find port for second
-                                               // lidar
     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
@@ -71,17 +48,15 @@ public class DriveTrain extends PIDSubsystem {
     leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
     rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
 
-    gyro = new GyroLib(I2C.Port.kOnboard, false);
-
-    DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
-    setEncoderPID();
     this.disable();
-    gyro.start();
 
-    leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_FORWARD,
-        +Constants.DriveTrain.LEFT_REVERSE);
-    rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.RIGHT_FORWARD,
-        Constants.DriveTrain.RIGHT_REVERSE);
+    leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.LEFT_SHIFT_MODULE,
+        Constants.DriveTrain.LEFT_SHIFT_FORWARD,
+        Constants.DriveTrain.LEFT_SHIFT_REVERSE);
+    rightGearPiston = new DoubleSolenoid(
+        Constants.DriveTrain.RIGHT_SHIFT_MODULE,
+        Constants.DriveTrain.RIGHT_SHIFT_FORWARD,
+        Constants.DriveTrain.RIGHT_SHIFT_REVERSE);
   }
 
   @Override
@@ -89,6 +64,7 @@ public class DriveTrain extends PIDSubsystem {
     setDefaultCommand(new JoystickDrive());
   }
 
+  // Print tne PID Output
   public void printOutput() {
     System.out.println("PIDOutput: " + pidOutput);
   }
@@ -97,6 +73,8 @@ public class DriveTrain extends PIDSubsystem {
     return (leftEncoder.getDistance() + rightEncoder.getDistance()) / 2;
   }
 
+  // Whether or not the PID Controller thinks we have reached the target
+  // setpoint
   public boolean reachedTarget() {
     if (this.onTarget()) {
       this.disable();
@@ -107,7 +85,7 @@ public class DriveTrain extends PIDSubsystem {
   }
 
   public void stop() {
-    drive(0, 0);
+    setMotorSpeeds(0, 0);
   }
 
   public void resetEncoders() {
@@ -115,14 +93,6 @@ public class DriveTrain extends PIDSubsystem {
     rightEncoder.reset();
   }
 
-  public double getLeftLidarDistance() {
-    return leftLidar.pidGet();
-  }
-
-  public double getRightLidarDistance() {
-    return rightLidar.pidGet();
-  }
-
   public double getRightSpeed() {
     return rightEncoder.getRate(); // in inches per second
   }
@@ -143,19 +113,10 @@ public class DriveTrain extends PIDSubsystem {
     return leftEncoder.getDistance(); // in inches
   }
 
+  // Get error between the setpoint of PID Controller and the current state of
+  // the robot
   public double getError() {
-    if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
-      return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
-    else
-      return Math.abs(this.getSetpoint() + getGyroAngle());
-  }
-
-  public double getGyroAngle() {
-    return gyro.getRotationZ().getAngle();
-  }
-
-  public void resetGyro() {
-    gyro.reset();
+    return Math.abs(this.getSetpoint() - getAvgEncoderDistance());
   }
 
   public void printEncoder(int i, int n) {
@@ -166,19 +127,17 @@ public class DriveTrain extends PIDSubsystem {
     }
   }
 
-  public void printGyroOutput() {
-    System.out.println("Gyro Angle" + -this.getGyroAngle());
-  }
-
+  /*
+   * returns the PID output that is returned by the PID Controller
+   */
   public double getOutput() {
     return pidOutput;
   }
 
+  // Updates the PID constants based on which control mode is being used
   public void updatePID() {
-    if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
-      this.getPIDController().setPID(kp, ki, kd);
-    else
-      this.getPIDController().setPID(gp, gd, gi);
+    this.getPIDController().setPID(Constants.DriveTrain.kp,
+        Constants.DriveTrain.ki, Constants.DriveTrain.kd);
   }
 
   public CANTalon getFrontLeft() {
@@ -197,25 +156,25 @@ public class DriveTrain extends PIDSubsystem {
     return rearRight;
   }
 
-  public int getMode() {
-    return DRIVE_MODE;
-  }
-
+  /*
+   * Method is a required method that the PID Subsystem uses to return the
+   * calculated PID value to the driver
+   * 
+   * @param Gives the user the output from the PID algorithm that is calculated
+   * internally
+   * 
+   * Body: Uses the output, does some filtering and drives the robot
+   */
   @Override
   protected void usePIDOutput(double output) {
     double left = 0;
     double right = 0;
-    if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) {
-      double drift = this.getLeftDistance() - this.getRightDistance();
-      if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
-        output = Math.signum(output) * 0.3;
-      left = output;
-      right = output + drift * kp / 10;
-    } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
-      left = output;
-      right = -output;
-    }
-    drive(left, right);
+    double drift = this.getLeftDistance() - this.getRightDistance();
+    if (Math.abs(output) > 0 && Math.abs(output) < 0.3)
+      output = Math.signum(output) * 0.3;
+    left = output;
+    right = output + drift * Constants.DriveTrain.kp / 10;
+    setMotorSpeeds(left, right);
     pidOutput = output;
   }
 
@@ -224,80 +183,98 @@ public class DriveTrain extends PIDSubsystem {
     return sensorFeedback();
   }
 
+  /*
+   * Checks the drive mode
+   * 
+   * @return the current state of the robot in each state Average distance from
+   * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE
+   */
   private double sensorFeedback() {
-    if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE)
-      return getAvgEncoderDistance();
-    else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE)
-      return -this.getGyroAngle();
-    // counterclockwise is positive on joystick but we want it to be negative
-    else
-      return 0;
+    return getAvgEncoderDistance();
   }
 
-  public void drive(double left, double right) {
-    robotDrive.tankDrive(-left, -right);
-    // dunno why but inverted drive (- values is forward)
-  }
+  public void joystickDrive(double left, double right) {
+    int type = Constants.DriveTrain.DRIVE_TYPE;
 
-  public void driveDistance(double dist, double maxTimeOut) {
-    dist = MathLib.constrain(dist, -100, 100);
-    setEncoderPID();
-    setSetpoint(dist);
-  }
-
-  public void setEncoderPID() {
-    DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE;
-    this.updatePID();
-    this.setAbsoluteTolerance(encoderTolerance);
-    this.setOutputRange(-1.0, 1.0);
-    this.setInputRange(-200.0, 200.0);
-    this.enable();
-  }
+    // Handle flipping of the "front" of the robot
+    double k = (isFlipped() ? -1 : 1);
 
-  private void setGyroPID() {
-    DRIVE_MODE = Constants.DriveTrain.GYRO_MODE;
-    this.updatePID();
-    this.getPIDController().setPID(gp, gi, gd);
-
-    this.setAbsoluteTolerance(gyroTolerance);
-    this.setOutputRange(-1.0, 1.0);
-    this.setInputRange(-360.0, 360.0);
-    this.enable();
-  }
+    robotDrive.tankDrive(-left, -right);
 
-  public void turnAngle(double angle) {
-    angle = MathLib.constrain(angle, -360, 360);
-    setGyroPID();
-    setSetpoint(angle);
+    // if (type == Constants.DriveTrain.TANK) {
+    // // TODO Test this for negation and for voltage vs. [-1,1] outputs
+    // double leftSpeed = (-frontLeft.get() + -rearLeft.get()) / 2;
+    // double rightSpeed = (-frontRight.get() + -rearRight.get()) / 2;
+    // left = ((Constants.DriveTrain.kADJUST - 1) * leftSpeed + left)
+    // / Constants.DriveTrain.kADJUST;
+    // right = ((Constants.DriveTrain.kADJUST - 1) * rightSpeed + right)
+    // / Constants.DriveTrain.kADJUST;
+    // robotDrive.tankDrive(-left * k, -right * k);
+    // } else if (type == Constants.DriveTrain.ARCADE) {
+    // double speed = (-frontLeft.get() + -rearLeft.get() + -frontRight.get() +
+    // -rearRight
+    // .get()) / 2;
+    // left = ((Constants.DriveTrain.kADJUST - 1) * speed + left)
+    // / Constants.DriveTrain.kADJUST;
+    // robotDrive.arcadeDrive(left * k, right);
+    // }
   }
 
   public void setMotorSpeeds(double left, double right) {
-    // positive setpoint to left side makes it go backwards
-    // positive setpoint to right side makes it go forwards.
-    frontLeft.set(-left);
-    rearLeft.set(-left);
-    frontRight.set(right);
-    rearRight.set(right);
-  }
-
-  public Value getLeftGearPistonValue() {
-    return leftGearPiston.get();
+    double k = (isFlipped() ? -1 : 1);
+    robotDrive.tankDrive(-left, -right);
   }
 
-  public Value getRightGearPistonValue() {
-    return rightGearPiston.get();
+  /**
+   * @return a value that is the current setpoint for the piston (kReverse or
+   *         kForward)
+   */
+  public Value getGearPistonValue() {
+    return leftGearPiston.get(); // Pistons should always be in the same state
   }
 
+  /**
+   * Changes the ball shift gear assembly to high
+   */
   public void setHighGear() {
     changeGear(Constants.DriveTrain.HIGH_GEAR);
   }
 
+  /**
+   * Changes the ball shift gear assembly to low
+   */
   public void setLowGear() {
     changeGear(Constants.DriveTrain.LOW_GEAR);
   }
 
+  /**
+   * Changes the gear to a DoubleSolenoid.Value
+   */
   public void changeGear(DoubleSolenoid.Value gear) {
     leftGearPiston.set(gear);
     rightGearPiston.set(gear);
   }
+
+  /**
+   * Switches drivetrain gears from high to low or low to high
+   */
+  public void switchGear() {
+    Value currentValue = getGearPistonValue();
+    Value setValue = (currentValue == Constants.DriveTrain.HIGH_GEAR) ? Constants.DriveTrain.LOW_GEAR
+        : Constants.DriveTrain.HIGH_GEAR;
+    changeGear(setValue);
+  }
+
+  /**
+   * Toggle whether the motor outputs are flipped, effectively switching which
+   * side of the robot is the front.
+   */
+  public void toggleFlipped() {
+    outputFlipped = !outputFlipped;
+  }
+
+  public boolean isFlipped() {
+    return outputFlipped;
+  }
+
 }