Make code for limit switch when hit will stop the arm

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel.java
new file mode 100644 (file)
index 0000000..a3b77ac
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel.java
@@ -0,0 +1,33 @@
+package org.usfirst.frc3501.RiceCatRobot.commands;
+
+import org.usfirst.frc3501.RiceCatRobot.subsystems.Arm;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class MoveArmToLevel extends Command {
+       Arm arm;
+       double slowSpeed = 0.2; //Slow speed to reach a limit switch
+       
+       public MoveArmToLevel(double leveldesired) {
+               arm.setArmSpeeds(slowSpeed); //Moves the arm to a certain slow speed
+       }
+
+    protected void initialize() {
+        arm.initializeCounter();
+    }
+
+    protected void execute() {
+    }
+
+    protected boolean isFinished() {
+        return arm.isSwitchHit();
+    }
+
+    protected void end() {
+        arm.stop(); //stops arm once limit switch hit
+    }
+
+    protected void interrupted() {
+        end();
+    }
+}

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java
index 8d78c8f48e9e8ccac467c6617c1cc3368cb38eba..50322e52d091cf061077d82eb31cbae2d57d85d0 100644 (file)
--- a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java
+++ b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java
@@ -3,11 +3,16 @@ package org.usfirst.frc3501.RiceCatRobot.subsystems;
 import org.usfirst.frc3501.RiceCatRobot.RobotMap;
 
 import edu.wpi.first.wpilibj.CANJaguar;
+import edu.wpi.first.wpilibj.Counter;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.command.Subsystem;
 
 public class Arm extends Subsystem {
   private CANJaguar left, right;
 
+  DigitalInput limitSwitch = new DigitalInput(1);
+  Counter counter = new Counter(limitSwitch);
+  
   public Arm() {
     left = new CANJaguar(RobotMap.ARM_LEFT);
     right = new CANJaguar(RobotMap.ARM_RIGHT);
@@ -15,6 +20,14 @@ public class Arm extends Subsystem {
 
   public void initDefaultCommand() {
   }
+  
+  public boolean isSwitchHit() {
+      return counter.get() > 0;
+  }
+  
+  public void initializeCounter() {
+      counter.reset();
+  }
 
   public void fineTuneControl(double d) {
     if (Math.abs(d) < 0.05) {