LED's and belt bug fix

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java

@@ -1,5 +1,7 @@
 package org.timecrafters.UltimateGoal.Competition;
 
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+
 import org.cyberarm.engine.V2.CyberarmState;
 
 public class Launch extends CyberarmState {
@@ -28,12 +30,15 @@ public class Launch extends CyberarmState {
         try {
             if (robot.stateConfiguration.action(groupName, actionName).enabled) {
                 robot.ringBeltMotor.setPower(0.5);
+                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_BLUE);
             } else {
                 setHasFinished(true);
             }
         } catch (NullPointerException e){
             robot.ringBeltMotor.setPower(0.5);
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_BLUE);
         }
+
     }
 
     @Override
@@ -66,9 +71,10 @@ public class Launch extends CyberarmState {
                 }
 
                 setHasFinished(true);
+                robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
             } else {
                 hasCycled = true;
-                reducePos = robot.loopPos((int) (beltPos + (1.5 * Robot.RING_BELT_GAP)));
+                reducePos = (int) (beltPos + (1.5 * Robot.RING_BELT_GAP));
             }
         }
         detectedPass = detectingPass;
@@ -76,6 +82,7 @@ public class Launch extends CyberarmState {
         boolean reduceCondition = (hasCycled && beltPos > reducePos);
         if (reduceCondition && !reduceConditionPrev){
             robot.ringBeltOn();
+
             //the ring belt stage lets other states know that the robot has finished launching all three rings
             //and is now returning to loading position.
 

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java

@@ -1,5 +1,7 @@
 package org.timecrafters.UltimateGoal.Competition;
 
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+
 import org.cyberarm.engine.V2.CyberarmState;
 
 public class ProgressRingBelt extends CyberarmState {
@@ -17,14 +19,16 @@ public class ProgressRingBelt extends CyberarmState {
     public void start() {
         int currentPos = robot.ringBeltMotor.getCurrentPosition();
         if (robot.ringBeltStage < 2) {
-            targetPos = robot.loopPos(currentPos + Robot.RING_BELT_GAP);
+            targetPos = currentPos + Robot.RING_BELT_GAP;
             robot.ringBeltOn();
             robot.ringBeltStage += 1;
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
         } else if (robot.ringBeltStage == 2) {
-            targetPos = robot.loopPos(currentPos + 160);
+            targetPos = currentPos + 160;
             robot.ringBeltOn();
             robot.ringBeltStage += 1;
             prepLaunch = !robot.initLauncher;
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
         } else if (robot.ringBeltStage > 2) {
             setHasFinished(true);
         }
@@ -34,27 +38,30 @@ public class ProgressRingBelt extends CyberarmState {
 
     @Override
     public void exec() {
-        if (robot.beltIsStuck() && childrenHaveFinished()) {
-            long currentTime = System.currentTimeMillis();
-            if (stuckStartTime == 0) {
-                stuckStartTime = currentTime;
-            } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) {
-                addParallelState(new UnstickRingBelt(robot));
-            }
-        } else {
-            stuckStartTime = 0;
-        }
 
         int currentPos = robot.ringBeltMotor.getCurrentPosition();
         if (currentPos >= targetPos) {
             robot.ringBeltMotor.setPower(0);
-
+            robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
             if(prepLaunch) {
                 robot.launchMotor.setPower(Robot.LAUNCH_POWER);
             }
 
             setHasFinished(true);
         }
+
+        if (robot.beltIsStuck() && childrenHaveFinished()) {
+            long currentTime = System.currentTimeMillis();
+            if (stuckStartTime == 0) {
+                stuckStartTime = currentTime;
+
+            } else if (currentTime - stuckStartTime >= robot.beltMaxStopTime) {
+
+                addParallelState(new UnstickRingBelt(robot));
+            }
+        } else {
+            stuckStartTime = 0;
+        }
     }
 
 }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java

@@ -5,6 +5,7 @@ import android.util.Log;
 
 import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.hardware.rev.RevTouchSensor;
 import com.qualcomm.robotcore.hardware.DcMotor;
@@ -49,6 +50,8 @@ public class Robot {
     public TimeCraftersConfiguration stateConfiguration = new TimeCraftersConfiguration();
     public BNO055IMU imu;
 
+    public RevBlinkinLedDriver ledDriver;
+
     //drive system
     public DcMotor driveFrontLeft;
     public DcMotor driveBackLeft;
@@ -286,6 +289,8 @@ public class Robot {
         launchRotation = stateConfiguration.variable("system", "launchPos","rot").value();
 
         initTensorFlow();
+
+        ledDriver = hardwareMap.get(RevBlinkinLedDriver.class, "leds");
     }
 
     private void initVuforia() {
@@ -628,11 +633,6 @@ public class Robot {
         return notMoved;
     }
 
-    public int loopPos(int pos) {
-        pos %= RING_BELT_LOOP_TICKS;
-        return pos;
-    }
-
     public void record(String record) {
         TestingRecord+="\n"+record;
     }

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java

@@ -1,5 +1,6 @@
 package org.timecrafters.UltimateGoal.Competition.TeleOp;
 
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
 import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.cyberarm.engine.V2.CyberarmState;
@@ -34,6 +35,11 @@ public class Player2 extends CyberarmState {
         robot.wobbleArmMotor.setPower(0.5);
     }
 
+    @Override
+    public void start() {
+        robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.DARK_GREEN);
+    }
+
     @Override
     public void exec() {
 

TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java

@@ -1,5 +1,7 @@
 package org.timecrafters.UltimateGoal.Competition;
 
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+
 import org.cyberarm.engine.V2.CyberarmState;
 
 public class UnstickRingBelt extends CyberarmState {
@@ -16,14 +18,15 @@ public class UnstickRingBelt extends CyberarmState {
     public void start() {
         lastPower = robot.ringBeltMotor.getPower();
         int currentPos = robot.ringBeltMotor.getCurrentPosition();
-        targetPos = robot.loopPos(currentPos - robot.beltReverseTicks);
+        targetPos = currentPos - robot.beltReverseTicks;
         robot.ringBeltMotor.setPower(-Robot.RING_BELT_POWER);
-
+        robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.HEARTBEAT_RED);
     }
 
     @Override
     public void exec() {
         int currentPos = robot.ringBeltMotor.getCurrentPosition();
+
         if (currentPos < targetPos) {
             robot.ringBeltMotor.setPower(lastPower);
             setHasFinished(true);

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LEDTest.java

@@ -0,0 +1,28 @@
+package org.timecrafters.UltimateGoal.HardwareTesting;
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.hardware.Servo;
+
+import org.cyberarm.engine.V2.CyberarmState;
+
+
+public class LEDTest extends CyberarmState {
+
+    RevBlinkinLedDriver leds;
+
+    @Override
+    public void init() {
+        leds = engine.hardwareMap.get(RevBlinkinLedDriver.class, "leds");
+    }
+
+    @Override
+    public void exec() {
+        setHasFinished(false);
+       if (engine.gamepad1.x) {
+           leds.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED);
+       } else {
+           leds.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN );
+       }
+    }
+
+}

TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java

@@ -10,21 +10,10 @@ import org.timecrafters.UltimateGoal.Competition.Robot;
 public class TestingEngine extends CyberarmEngine {
 
     private Robot robot;
-    @Override
-    public void init() {
-        robot = new Robot(hardwareMap);
-        robot.initHardware();
-        super.init();
-    }
 
     @Override
     public void setup() {
-        addState(new FindWobbleGoal(robot, "auto", "08_0"));
+        addState(new LEDTest());
     }
 
-//    @Override
-//    public void stop() {
-//        robot.saveRecording();
-//        super.stop();
-//    }
 }