add a min speed for the arm in move arm to angle command

[3501/stronghold-2016] / src / org / usfirst / frc / team3501 / robot / commands / intakearm / MoveIntakeArmToAngle.java

diff --git a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java
index f00e62e1984bb4dc49b682bc2b911c118f8c27b2..510146cc1a8bc7a425c29dda2d63ab7dbe7a5e32 100644 (file)
--- a/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java
+++ b/src/org/usfirst/frc/team3501/robot/commands/intakearm/MoveIntakeArmToAngle.java
@@ -10,6 +10,7 @@ public class MoveIntakeArmToAngle extends Command {
   private double targetSpeed;
   private double calculatedSpeed;
   private double SENSITIVITY_THRESHOLD = 0.1;
+  private double MIN_SPEED = .3;
 
   public MoveIntakeArmToAngle(double angle, double speed) {
     requires(Robot.intakeArm);
@@ -22,8 +23,11 @@ public class MoveIntakeArmToAngle extends Command {
   protected void initialize() {
 
     // set the arm speed to the calculated angle
-    Robot.intakeArm.setArmSpeed(getCalculatedSpeed());
+    double speed = getCalculatedSpeed();
 
+    if (speed < MIN_SPEED && speed > 0)
+      speed = Math.signum(getCalculatedSpeed()) * MIN_SPEED;
+    Robot.intakeArm.setArmSpeed(speed);
   }
 
   @Override