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 96f1d1150fd5d87a2d5e5531f8515fcf10302e4f..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
@@ -44,8 +46,7 @@ public class DriveTrain extends PIDSubsystem {
 
     robotDrive = new RobotDrive(frontLeft, rearLeft, frontRight, rearRight);
 
-    lidar = new Lidar(I2C.Port.kOnboard);
-
+    lidar = new Lidar(I2C.Port.kMXP);
     leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
         Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
     rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
@@ -63,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;
   }
 
@@ -197,10 +201,10 @@ public class DriveTrain extends PIDSubsystem {
   /*
    * 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
@@ -228,7 +232,7 @@ public class DriveTrain extends PIDSubsystem {
 
   /*
    * 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
    */
@@ -249,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);
@@ -257,12 +261,16 @@ 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
-   *
+   * 
    * Configure Encoder PID
-   *
+   * 
    * Sets the setpoint to the PID subsystem
    */
   public void driveDistance(double dist, double maxTimeOut) {
@@ -302,10 +310,10 @@ public class DriveTrain extends PIDSubsystem {
 
   /*
    * Turning method that should be used repeatedly in a command
-   *
+   * 
    * First constrains the angle to within -360 and 360 since that is as much as
    * we need to turn
-   *
+   * 
    * Configures Gyro PID and sets the setpoint as an angle
    */
   public void turnAngle(double angle) {
@@ -360,4 +368,20 @@ public class DriveTrain extends PIDSubsystem {
     leftGearPiston.set(gear);
     rightGearPiston.set(gear);
   }
+
+  public void startCompressor() {
+    compressor.start();
+  }
+
+  public void stopCompressor() {
+    compressor.stop();
+  }
+
+  public void toggleCompressor() {
+    if (compressor.enabled())
+      compressor.stop();
+    else
+      compressor.start();
+  }
+
 }