From be8b33f8578fc477dab7e0174dba9c6f1fcd1a76 Mon Sep 17 00:00:00 2001 From: Nathaniel Palme Date: Thu, 4 Mar 2021 20:20:05 -0600 Subject: [PATCH] LED's and belt bug fix --- .../UltimateGoal/Competition/Launch.java | 9 ++++- .../Competition/ProgressRingBelt.java | 33 +++++++++++-------- .../UltimateGoal/Competition/Robot.java | 10 +++--- .../Competition/TeleOp/Player2.java | 6 ++++ .../Competition/UnstickRingBelt.java | 7 ++-- .../UltimateGoal/HardwareTesting/LEDTest.java | 28 ++++++++++++++++ .../HardwareTesting/TestingEngine.java | 13 +------- 7 files changed, 73 insertions(+), 33 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LEDTest.java diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java index 180989e..02ecc42 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Launch.java +++ b/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. diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java index bc7c13f..bc3fd9f 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/ProgressRingBelt.java +++ b/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; + } } } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java index ab63650..3daa955 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/Robot.java +++ b/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; } diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java index bc35acc..67cd1af 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/TeleOp/Player2.java +++ b/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() { diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java index 7452b91..0f3187c 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/Competition/UnstickRingBelt.java +++ b/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); diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LEDTest.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LEDTest.java new file mode 100644 index 0000000..a6d28c9 --- /dev/null +++ b/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 ); + } + } + +} diff --git a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java b/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java index 9913ea9..3f92521 100644 --- a/TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java +++ b/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(); -// } }