add method to calculate angle to turn after passing through defense to shoot

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / subsystems / DriveTrain.java

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 81f80b92ffa3d7ccd670757b3444273ca8438580..0d1db15b930268ccea2a86b65627118080932e14 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -7,12 +7,16 @@ import org.usfirst.frc.team3501.robot.commands.driving.JoystickDrive;
 
 import edu.wpi.first.wpilibj.CANTalon;
 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;
@@ -20,10 +24,29 @@ public class DriveTrain extends PIDSubsystem {
   private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
 
   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);
@@ -34,6 +57,10 @@ public class DriveTrain extends PIDSubsystem {
     rearRight = new CANTalon(Constants.DriveTrain.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,
@@ -51,6 +78,10 @@ public class DriveTrain extends PIDSubsystem {
     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);
   }
 
   @Override
@@ -84,6 +115,14 @@ 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
   }
@@ -172,8 +211,7 @@ public class DriveTrain extends PIDSubsystem {
         output = Math.signum(output) * 0.3;
       left = output;
       right = output + drift * kp / 10;
-    }
-    else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
+    } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
       left = output;
       right = -output;
     }
@@ -242,6 +280,24 @@ public class DriveTrain extends PIDSubsystem {
     rearRight.set(right);
   }
 
-  private static double kp = 0.013, ki = 0.000015, kd = -0.002;
-  private static double gp = 0.018, gi = 0.000015, gd = 0;
+  public Value getLeftGearPistonValue() {
+    return leftGearPiston.get();
+  }
+
+  public Value getRightGearPistonValue() {
+    return rightGearPiston.get();
+  }
+
+  public void setHighGear() {
+    changeGear(Constants.DriveTrain.HIGH_GEAR);
+  }
+
+  public void setLowGear() {
+    changeGear(Constants.DriveTrain.LOW_GEAR);
+  }
+
+  public void changeGear(DoubleSolenoid.Value gear) {
+    leftGearPiston.set(gear);
+    rightGearPiston.set(gear);
+  }
 }