From d9c04720cc5bfc1cafab0c73e71dd231bbc65ff7 Mon Sep 17 00:00:00 2001
From: Shaina Chen <shaina.sierra@gmail.com>
Date: Sat, 13 Feb 2016 17:15:37 -0800
Subject: [PATCH] add two piston variables for shifting gears

---
 .../usfirst/frc/team3501/robot/Constants.java |  2 ++
 .../team3501/robot/subsystems/DriveTrain.java | 20 +++++++++++++++++++
 2 files changed, 22 insertions(+)

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index 8f121992..72349107 100644
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -42,6 +42,8 @@ public class Constants {
         ((3.66 / 5.14) * 6 * Math.PI) / 256;
 
     public static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
+    public static final int LEFT_FORWARD = 0, LEFT_REVERSE = 1,
+        RIGHT_FORWARD = 2, RIGHT_REVERSE = 3;
     public static double time = 0;
   }
 
diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 81f80b92..5e9f2bd9 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -7,6 +7,7 @@ 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.Encoder;
 import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.RobotDrive;
@@ -24,6 +25,21 @@ public class DriveTrain extends PIDSubsystem {
   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);
@@ -51,6 +67,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
-- 
2.30.2