Add compressor control code and fix solenoid ports

diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java
index 54ffd0091e4ec013cb727f03bc9a11a9e0a3d831..1a1a20bb5c2fbd0a45460a257aff2d2ef9b4594c 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Constants.java
+++ b/src/org/usfirst/frc/team3501/robot/Constants.java
@@ -37,6 +37,7 @@ public class Constants {
     public final static int RIGHT_JOYSTICK_THUMB_PORT = 2;
 
     public final static int TOGGLE_SCALING_PORT = 0;
+    public final static int TOGGLE_COMPRESSOR_PORT = 12;
 
   }
 
@@ -63,8 +64,12 @@ public class Constants {
     public static double encoderTolerance = 8.0, gyroTolerance = 5.0;
 
     public static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3;
-    public static final int LEFT_FORWARD = 1, LEFT_REVERSE = 2,
-        RIGHT_FORWARD = 9, RIGHT_REVERSE = 10;
+
+    public static final int MODULE_A_ID = 9, MODULE_B_ID = 10;
+    public static final int LEFT_FORWARD = 5, LEFT_REVERSE = 1,
+        RIGHT_FORWARD = 4, RIGHT_REVERSE = 0;
+    public static final int COMPRESSOR_ID = 9;
+
     public static double time = 0;
 
     // Gearing constants

diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java
index cb7e12d9b3120c094e24f39869be0d6deea94cf6..8de027b73502223533f8b786c11f1804909ca53a 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/OI.java
+++ b/src/org/usfirst/frc/team3501/robot/OI.java
@@ -1,6 +1,7 @@
 package org.usfirst.frc.team3501.robot;
 
 import org.usfirst.frc.team3501.robot.commands.driving.ChangeGear;
+import org.usfirst.frc.team3501.robot.commands.driving.ToggleCompressor;
 
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.buttons.Button;
@@ -34,6 +35,7 @@ public class OI {
   public static Button intakeBoulder;
   public static Button shootBoulder;
   public static Button toggleGear;
+  public static Button toggleCompressor;
 
   // button to change robot to the scaling mode
   public static DigitalButton toggleScaling;
@@ -46,6 +48,10 @@ public class OI {
         Constants.OI.RIGHT_JOYSTICK_TRIGGER_PORT);
     toggleGear.toggleWhenPressed(new ChangeGear());
 
+    toggleCompressor = new JoystickButton(rightJoystick,
+        Constants.OI.TOGGLE_COMPRESSOR_PORT);
+    toggleCompressor.whenPressed(new ToggleCompressor());
+
     // passPortcullis = new DigitalButton(
     // new DigitalInput(Constants.OI.PASS_PORTCULLIS_PORT));
     // passPortcullis.whenPressed(new PassPortcullis());

diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java
index 5bb61f143bc139c14b81221919ef90893b4c0382..14f86db3be6c821014c7dc88c6b8eeaf2cb861c0 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/Robot.java
+++ b/src/org/usfirst/frc/team3501/robot/Robot.java
@@ -121,6 +121,7 @@ public class Robot extends IterativeRobot {
   @Override
   public void teleopInit() {
     Robot.driveTrain.setLowGear();
+    Robot.driveTrain.startCompressor();
   }
 
   @Override

diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java
index ac239361d2d44edc83358f8bb15dd54b1320cc56..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,11 +64,13 @@ public class DriveTrain extends PIDSubsystem {
     this.disable();
     gyro.start();
 
-    leftGearPiston = new DoubleSolenoid(10, Constants.DriveTrain.LEFT_FORWARD,
-        Constants.DriveTrain.LEFT_REVERSE);
-    rightGearPiston = new DoubleSolenoid(10,
-        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
@@ -213,12 +217,11 @@ public class DriveTrain extends PIDSubsystem {
         output = Math.signum(output) * 0.3;
       left = output;
       right = output + drift * Constants.DriveTrain.kp / 10;
-      drive(left, right);
     } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) {
       left = output;
       right = -output;
-      arcadeDrive(0, output);
     }
+    drive(left, right);
     pidOutput = output;
   }
 
@@ -229,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
    */
@@ -265,9 +268,9 @@ public class DriveTrain extends PIDSubsystem {
   /*
    * 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) {
@@ -307,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) {
@@ -366,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();
+  }
+
 }