From: Kevin Zhang <icestormf1@gmail.com>
Date: Mon, 15 Feb 2016 00:10:37 +0000 (-0800)
Subject: Add hig/low gear changing methods and corresponding constants in master
X-Git-Url: http://challenge-bot.com/repos/?p=3501%2Fstronghold-2016;a=commitdiff_plain;h=2a099bc6e824accc4ea0df2db2f6af666cf9aad4

Add hig/low gear changing methods and corresponding constants in master
---

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index fd77fdae..e12b1e44 100644
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -1,6 +1,7 @@
 package org.usfirst.frc.team3501.robot;
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
 
 /**
  * The Constants stores constant values for all subsystems. This includes the
@@ -48,6 +49,11 @@ public class Constants {
     public static final int LEFT_FORWARD = 0, LEFT_REVERSE = 1,
         RIGHT_FORWARD = 2, RIGHT_REVERSE = 3;
     public static double time = 0;
+
+    // Gearing constants
+    public static final Value HIGH_GEAR = DoubleSolenoid.Value.kForward;
+    public static final Value LOW_GEAR = DoubleSolenoid.Value.kReverse;
+
   }
 
   public static class Scaler {
diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index 56d8066b..de2626be 100644
--- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
+++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
@@ -15,6 +15,8 @@ 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;
@@ -40,7 +42,7 @@ public class DriveTrain extends PIDSubsystem {
   // 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;
+          / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
 
   public DriveTrain() {
     super(kp, ki, kd);
@@ -105,14 +107,6 @@ public class DriveTrain extends PIDSubsystem {
     rightEncoder.reset();
   }
 
-  public Value getLeftGearPistonValue() {
-    return leftGearPiston.get();
-  }
-
-  public Value getRightGearPistonValue() {
-    return rightGearPiston.get();
-  }
-
   public double getRightSpeed() {
     return rightEncoder.getRate(); // in inches per second
   }
@@ -271,6 +265,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);
+  }
 }