Uses arrays and sends message when a specific limitswitch is hit

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/LimitSwitchTest.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/LimitSwitchTest.java
new file mode 100644 (file)
index 0000000..daedb3c
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/LimitSwitchTest.java
@@ -0,0 +1,15 @@
+package org.usfirst.frc3501.RiceCatRobot.commands;
+
+import edu.wpi.first.wpilibj.command.CommandGroup;
+
+public class LimitSwitchTest extends CommandGroup {
+       /**
+        * It is necessary for all commands to be used. From MoveArmToLevel1 - MoveArmToLevel3
+        */
+       
+       public LimitSwitchTest() {
+               addSequential( new MoveArmToLevel1() );
+               addSequential( new MoveArmToLevel2() );
+               addSequential( new MoveArmToLevel3() );
+       }
+}

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel1.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel1.java
new file mode 100644 (file)
index 0000000..158a4be
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel1.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 MoveArmToLevel1 extends Command {
+       Arm arm;
+       double slowSpeed = 0.2;
+
+       public MoveArmToLevel1() {
+               arm.setArmSpeeds(slowSpeed);
+       }
+
+       protected void initialize() {
+               arm.initializeCounters();
+       }
+
+       protected void execute() {
+       }
+
+       protected boolean isFinished() {
+               return arm.isSwitch1Hit();
+       }
+
+       protected void end() {
+               System.out.println("Robot arm has reached level 1");
+       }
+
+       protected void interrupted() {
+               end();
+       }
+}

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel2.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel2.java
new file mode 100644 (file)
index 0000000..4152739
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel2.java
@@ -0,0 +1,34 @@
+package org.usfirst.frc3501.RiceCatRobot.commands;
+
+import org.usfirst.frc3501.RiceCatRobot.subsystems.Arm;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class MoveArmToLevel2 extends Command {
+       Arm arm;
+       double slowSpeed = 0.2;
+       public MoveArmToLevel2() {
+               if(arm.getArmSpeed() == 0.0) {
+                       arm.setArmSpeeds(slowSpeed);
+               }
+       }
+
+       protected void initialize() {
+               arm.initializeCounters();
+       }
+
+       protected void execute() {
+       }
+
+       protected boolean isFinished() {
+               return arm.isSwitch2Hit();
+       }
+
+       protected void end() {
+               System.out.println("Robot arm has reached level 2");
+       }
+
+       protected void interrupted() {
+               end();
+       }
+}

diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel3.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel3.java
new file mode 100644 (file)
index 0000000..d969dee
--- /dev/null
+++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel3.java
@@ -0,0 +1,37 @@
+package org.usfirst.frc3501.RiceCatRobot.commands;
+
+import org.usfirst.frc3501.RiceCatRobot.subsystems.Arm;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class MoveArmToLevel3 extends Command {
+       Arm arm;
+
+       double slowSpeed = 0.2;
+
+       public MoveArmToLevel3() {
+               if (arm.getArmSpeed() == 0.0) {
+                       arm.setArmSpeeds(slowSpeed);
+               }
+       }
+
+       protected void initialize() {
+               arm.initializeCounters();
+       }
+
+       protected void execute() {
+       }
+
+       protected boolean isFinished() {
+               return arm.isSwitch3Hit();
+       }
+
+       protected void end() {
+               System.out.println("Robot arm has reached level 3");
+               arm.stop();
+       }
+
+       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..a29262a37d588133326f94f510c894185886dc42 100644 (file)
--- a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java
+++ b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java
@@ -3,45 +3,87 @@ 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;
+       private CANJaguar left, right;
+       double speedOfArm = 0.0;
 
-  public Arm() {
-    left = new CANJaguar(RobotMap.ARM_LEFT);
-    right = new CANJaguar(RobotMap.ARM_RIGHT);
-  }
+       DigitalInput[] limitSwitches = new DigitalInput[3];
+       Counter[] counters = new Counter[3];
 
-  public void initDefaultCommand() {
-  }
+       public Arm() {
+               left = new CANJaguar(RobotMap.ARM_LEFT);
+               right = new CANJaguar(RobotMap.ARM_RIGHT);
+       }
 
-  public void fineTuneControl(double d) {
-    if (Math.abs(d) < 0.05) {
-      d = 0;
-    } else if (d > 0) {
-      d *= d;
-    } else {
-      d *= -d;
-    }
-    setArmSpeeds(d);
-  }
+       public void setDigitalInputValues() {
+               limitSwitches[0] = new DigitalInput(0);
+               limitSwitches[1] = new DigitalInput(1);
+               limitSwitches[2] = new DigitalInput(2);
+       }
 
-  public void setLeft(double speed) {
-    left.set(-speed);
-  }
+       public void setCountersCorrespondingToSwitches() {
+               counters[0] = new Counter(0);
+               counters[1] = new Counter(1);
+               counters[3] = new Counter(2);
+       }
 
-  public void setRight(double speed) {
-    right.set(-speed);
+       public double getArmSpeed() {
+         return this.speedOfArm;
   }
 
-  public void setArmSpeeds(double speed) {
-    setLeft(speed);
-    setRight(speed);
-  }
+       public boolean isSwitch1Hit() {
+               return counters[0].get() > 0;
+       }
 
-  public void stop() {
-    left.set(0);
-    right.set(0);
-  }
+       public boolean isSwitch2Hit() {
+               return counters[1].get() > 0;
+       }
+
+       public boolean isSwitch3Hit() {
+               return counters[2].get() > 0;
+       }
+
+       public void initializeCounters() {
+               for (int i = 0; i < counters.length; i++) {
+                       counters[i].reset();
+               }
+       }
+
+       public void initDefaultCommand() {
+       }
+
+       public void fineTuneControl(double d) {
+               if (Math.abs(d) < 0.05) {
+                       d = 0;
+               } else if (d > 0) {
+                       d *= d;
+               } else {
+                       d *= -d;
+               }
+               setArmSpeeds(d);
+       }
+
+       public void setLeft(double speed) {
+               left.set(-speed);
+       }
+
+       public void setRight(double speed) {
+               right.set(-speed);
+       }
+
+       public void setArmSpeeds(double speed) {
+               this.speedOfArm = speed;
+               setLeft(speed);
+               setRight(speed);
+       }
+
+       public void stop() {
+               this.speedOfArm = 0.0;
+               left.set(0);
+               right.set(0);
+       }
 }