Add compressor control code and fix solenoid ports

[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 203078ccfd7f88d9103b8a6ddf4ac529b6212c4c..f5a7d6c21eaffb98ea258c4cedc4061188c79b1f 100644 (file)
--- 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.sensors.GyroLib;
 import org.usfirst.frc.team3501.robot.sensors.Lidar;
 
 import edu.wpi.first.wpilibj.CANTalon;
+import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.CounterBase.EncodingType;
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
@@ -29,6 +30,7 @@ public class DriveTrain extends PIDSubsystem {
 
   private GyroLib gyro;
   private DoubleSolenoid leftGearPiston, rightGearPiston;
+  private Compressor compressor;
 
   // Drivetrain specific constants that relate to the inches per pulse value for
   // the encoders
@@ -62,10 +64,13 @@ 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);
+    leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_B_ID,
+        Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE);
+    rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_B_ID,
+        Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE);
+
+    compressor = new Compressor(Constants.DriveTrain.COMPRESSOR_ID);
+
     Constants.DriveTrain.inverted = false;
   }
 
@@ -248,7 +253,7 @@ public class DriveTrain extends PIDSubsystem {
    * because RobotDrive tankdrive method drives inverted
    */
   public void drive(double left, double right) {
-    robotDrive.tankDrive(-left, -right);
+//    robotDrive.tankDrive(-left, -right);
     // dunno why but inverted drive (- values is forward)
     if (!Constants.DriveTrain.inverted)
       robotDrive.tankDrive(-left, -right);
@@ -256,6 +261,10 @@ public class DriveTrain extends PIDSubsystem {
       robotDrive.tankDrive(right, left);
   }
 
+  public void arcadeDrive(double y, double twist) {
+    robotDrive.arcadeDrive(y, twist);
+  }
+
   /*
    * constrains the distance to within -100 and 100 since we aren't going to
    * drive more than 100 inches
@@ -360,4 +369,19 @@ public class DriveTrain extends PIDSubsystem {
     rightGearPiston.set(gear);
   }
 
+  public void startCompressor() {
+    compressor.start();
+  }
+
+  public void stopCompressor() {
+    compressor.stop();
+  }
+
+  public void toggleCompressor() {
+    if (compressor.enabled())
+      compressor.stop();
+    else
+      compressor.start();
+  }
+
 }