Uses arrays and sends message when a specific limitswitch is hit

[3501/2015-FRC-Spark] / src / org / usfirst / frc3501 / RiceCatRobot / subsystems / Arm.java

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);
+       }
 }