diff --git a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java index b96d034..6dbc79e 100644 --- a/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java +++ b/TeamCode/src/main/java/org/cyberarm/engine/V2/CyberarmEngine.java @@ -5,6 +5,8 @@ import android.util.Log; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.Gamepad; +import java.util.ArrayList; +import java.util.HashMap; import java.util.concurrent.CopyOnWriteArrayList; /** @@ -17,7 +19,8 @@ public abstract class CyberarmEngine extends OpMode { public static CyberarmEngine instance; //Array To Hold States final private CopyOnWriteArrayList cyberarmStates = new CopyOnWriteArrayList<>(); - private int activeStateIndex = 0; + public HashMap blackboard = new HashMap<>(); + private int activeStateIndex = 0; private boolean isRunning; private final static String TAG = "PROGRAM.ENGINE"; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java new file mode 100644 index 0000000..fabe850 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftSideAutonomousEngine.java @@ -0,0 +1,96 @@ +package org.timecrafters.Autonomous.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.Autonomous.States.BottomArm; +import org.timecrafters.Autonomous.States.CollectorDistanceState; +import org.timecrafters.Autonomous.States.CollectorState; +import org.timecrafters.Autonomous.States.ConeIdentification; +import org.timecrafters.Autonomous.States.DriverParkPlaceState; +import org.timecrafters.Autonomous.States.DriverState; +import org.timecrafters.Autonomous.States.PathDecision; +import org.timecrafters.Autonomous.States.RotationState; +import org.timecrafters.Autonomous.States.TopArm; +import org.timecrafters.testing.states.PrototypeBot1; + +@Autonomous (name = "Left Side") + +public class LeftSideAutonomousEngine extends CyberarmEngine { + PrototypeBot1 robot; + + @Override + public void setup() { + robot = new PrototypeBot1(this); + addState(new ConeIdentification(robot, "LeftSideAutonomous", "00-0")); + //drive to high pole + addState(new DriverState(robot, "LeftSideAutonomous", "01-0")); + //turn towards high pole + addState(new RotationState(robot, "LeftSideAutonomous", "02-0")); + //lift the upper arm + addState(new TopArm(robot, "LeftSideAutonomous", "03-0")); + //lift the lower arm + addState(new BottomArm(robot, "LeftSideAutonomous", "04-0")); + //drive forward to junction + addState(new DriverState(robot, "LeftSideAutonomous", "05-0")); + //lower the bottom arm to get closer + addState(new TopArm(robot, "LeftSideAutonomous", "06-0")); + //make collector release the cone + addState(new CollectorState(robot, "LeftSideAutonomous", "07-0")); + //lift the lower arm to clear the pole + addState(new TopArm(robot, "LeftSideAutonomous", "08-0")); + //Drive Backwards away from junction + addState(new DriverState(robot, "LeftSideAutonomous", "09-0")); + // Rotate to either set up for cones to grab or park somewhere + addState(new RotationState(robot, "LeftSideAutonomous", "10-0")); + // lower the bottom arm so we dont fall over + addState(new BottomArm(robot, "LeftSideAutonomous", "11-0")); + // lower the upper arm so we dont fall over + addState(new TopArm(robot, "LeftSideAutonomous", "12-0"));; + //adjust arm height to cone. + addState(new TopArm(robot, "LeftSideAutonomous", "13-0")); + //drive towards stack of cones while collecting + addState(new CollectorDistanceState(robot, "LeftSideAutonomous", "14-0")); + //drive slightly back + addState(new DriverState(robot, "LeftSideAutonomous", "15-0")); + // face to -90 + addState(new RotationState(robot, "LeftSideAutonomous", "15-1")); + //lift arm up + addState(new TopArm(robot, "LeftSideAutonomous", "16-0")); + // drive backwards too position + addState(new DriverState(robot, "LeftSideAutonomous", "17-0")); + // face low junction + addState(new RotationState(robot, "LeftSideAutonomous", "18-0")); + // drive up to junction + addState(new DriverState(robot, "LeftSideAutonomous", "19-0")); + // bring arm down. + addState(new TopArm(robot, "LeftSideAutonomous", "20-0")); + // get rid of cone + addState(new CollectorState(robot, "LeftSideAutonomous", "21-0")); + // lift arm up to clear + addState(new TopArm(robot, "LeftSideAutonomous", "22-0")); + // rotate towards stack of cones + addState(new RotationState(robot, "LeftSideAutonomous", "23-0")); + // Choose Parking Spot + addState(new PathDecision(robot, "LeftSideAutonomous", "24-0")); + // case 1 drive forward + addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-1")); + // case 2 drive forward + addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-2")); + // case 3 drive forward + addState(new DriverParkPlaceState(robot, "LeftSideAutonomous", "24-3")); + // zero out + addState(new RotationState(robot, "LeftSideAutonomous", "25-0")); + // Top Arm Down + addState(new TopArm(robot, "LeftSideAutonomous", "25-1")); + //Drive Backwards + addState(new DriverState(robot, "LeftSideAutonomous", "26-0")); + } + + @Override + public void loop() { + super.loop(); + + telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java new file mode 100644 index 0000000..5bcd405 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightSideAutonomousEngine.java @@ -0,0 +1,98 @@ +package org.timecrafters.Autonomous.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.Autonomous.States.CollectorDistanceState; +import org.timecrafters.Autonomous.States.CollectorState; +import org.timecrafters.Autonomous.States.ConeIdentification; +import org.timecrafters.Autonomous.States.DriverParkPlaceState; +import org.timecrafters.Autonomous.States.DriverState; +import org.timecrafters.Autonomous.States.BottomArm; +import org.timecrafters.Autonomous.States.PathDecision; +import org.timecrafters.Autonomous.States.RotationState; +import org.timecrafters.Autonomous.States.TopArm; +import org.timecrafters.testing.states.PrototypeBot1; + +@Autonomous (name = "Right Side") + +public class RightSideAutonomousEngine extends CyberarmEngine { + PrototypeBot1 robot; + + @Override + public void setup() { + robot = new PrototypeBot1(this); + addState(new ConeIdentification(robot, "RightSideAutonomous", "00-0")); + //drive to high pole + addState(new DriverState(robot, "RightSideAutonomous", "01-0")); + //turn towards high pole + addState(new RotationState(robot, "RightSideAutonomous", "02-0")); + //lift the upper arm + addState(new TopArm(robot, "RightSideAutonomous", "03-0")); + //lift the lower arm + addState(new BottomArm(robot, "RightSideAutonomous", "04-0")); + //drive forward + addState(new DriverState(robot, "RightSideAutonomous", "05-0")); + // drive backward + addState(new DriverState(robot, "RightSideAutonomous", "05-1")); + //lower the bottom arm to get closer + addState(new TopArm(robot, "RightSideAutonomous", "06-0")); + //make collector release the cone + addState(new CollectorState(robot, "RightSideAutonomous", "07-0")); + //lift the lower arm to clear the pole + addState(new TopArm(robot, "RightSideAutonomous", "08-0")); + //Drive Backwards + addState(new DriverState(robot, "RightSideAutonomous", "09-0")); + // lower the bottom arm so we dont fall over + addState(new BottomArm(robot, "RightSideAutonomous", "10-0")); + // lower the upper arm so we dont fall over + addState(new TopArm(robot, "RightSideAutonomous", "11-0"));; + // Rotate to either set up for cones to grab or park somewhere + addState(new RotationState(robot, "RightSideAutonomous", "12-0")); + //adjust arm height to cone. + addState(new TopArm(robot, "RightSideAutonomous", "13-0")); + //drive towards stack of cones while collecting + addState(new CollectorDistanceState(robot, "RightSideAutonomous", "14-0")); + //drive slightly back + addState(new DriverState(robot, "RightSideAutonomous", "15-0")); + // face to -90 + addState(new RotationState(robot, "RightSideAutonomous", "15-1")); + //lift arm up + addState(new TopArm(robot, "RightSideAutonomous", "16-0")); + // drive backwards too position + addState(new DriverState(robot, "RightSideAutonomous", "17-0")); + // rotate + addState(new RotationState(robot, "RightSideAutonomous", "18-0")); + // drive slightly towards junction + addState(new DriverState(robot, "RightSideAutonomous", "18-1")); + // drive away from junction + addState(new DriverState(robot, "RightSideAutonomous", "18-2")); + // bring arm down. + addState(new TopArm(robot, "RightSideAutonomous", "19-0")); + // get rid of cone + addState(new CollectorState(robot, "RightSideAutonomous", "20-0")); + // lift arm up to clear + addState(new TopArm(robot, "RightSideAutonomous", "21-0")); + // rotate towards stack of cones + addState(new RotationState(robot, "RightSideAutonomous", "22-0")); + // Choose Parking Spot + addState(new PathDecision(robot, "RightSideAutonomous", "23-0")); + // case 1 drive forward + addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-1")); + // case 2 drive forward + addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-2")); + // case 3 drive forward + addState(new DriverParkPlaceState(robot, "RightSideAutonomous", "23-3")); + // zero out + addState(new RotationState(robot, "RightSideAutonomous", "24-0")); + // Top Arm Down + addState(new TopArm(robot, "RightSideAutonomous", "25-0")); + } + + @Override + public void loop() { + super.loop(); + + telemetry.addData("BlackBoard Input", blackboard.get("parkPlace")); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java deleted file mode 100644 index cca4e0b..0000000 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/TestAutonomousEngine.java +++ /dev/null @@ -1,66 +0,0 @@ -package org.timecrafters.Autonomous.Engines; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.hardware.DcMotor; - -import org.cyberarm.engine.V2.CyberarmEngine; -import org.timecrafters.Autonomous.States.CollectorDistanceState; -import org.timecrafters.Autonomous.States.CollectorState; -import org.timecrafters.Autonomous.States.DriverState; -import org.timecrafters.Autonomous.States.BottomArm; -import org.timecrafters.Autonomous.States.RotationState; -import org.timecrafters.Autonomous.States.TopArm; -import org.timecrafters.testing.states.PrototypeBot1; - -@Autonomous (name = "Autonomous Test") - -public class TestAutonomousEngine extends CyberarmEngine { - PrototypeBot1 robot; - - @Override - public void setup() { - robot = new PrototypeBot1(this); - - //drive to high pole - addState(new DriverState(robot, "TestAutonomous", "01-0")); - //turn towards high pole - addState(new RotationState(robot, "TestAutonomous", "02-0")); - //lift the upper arm - addState(new TopArm(robot, "TestAutonomous", "03-0")); - //lift the lower arm - addState(new BottomArm(robot, "TestAutonomous", "04-0")); - //drive forward - addState(new DriverState(robot, "TestAutonomous", "05-0")); - //lower the bottom arm to get closer - addState(new TopArm(robot, "TestAutonomous", "06-0")); - //make collector release the cone - addState(new CollectorState(robot, "TestAutonomous", "07-0")); - //lift the lower arm to clear the pole - addState(new TopArm(robot, "TestAutonomous", "08-0")); - //Drive Backwards - addState(new DriverState(robot, "TestAutonomous", "09-0")); - // lower the bottom arm so we dont fall over - addState(new BottomArm(robot, "TestAutonomous", "10-0")); - // lower the upper arm so we dont fall over - addState(new TopArm(robot, "TestAutonomous", "11-0"));; - // Rotate to either set up for cones to grab or park somewhere - addState(new RotationState(robot, "TestAutonomous", "12-0")); - //adjust arm height to cone. - addState(new TopArm(robot, "TestAutonomous", "13-0")); - //drive towards stack of cones - addState(new CollectorDistanceState(robot, "", "")); -// DriverState driverState = new DriverState(robot, "TestAutonomous", "14-0"); -// addState(driverState); -// //collect next cone -// driverState.addParallelState(new CollectorState(robot, "TestAutonomous", "15-0")); - - - - - - - - - - } -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java index 0865724..6c74a5c 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/BottomArm.java @@ -11,6 +11,7 @@ public class BottomArm extends CyberarmState { long time; long lastStepTime = 0; boolean up; + boolean directPosition; public BottomArm(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; @@ -18,6 +19,7 @@ public class BottomArm extends CyberarmState { this.LowerRiserRightPos = robot.configuration.variable(groupName, actionName, "LowerRiserRightPos").value(); this.time = robot.configuration.variable(groupName, actionName, "time").value(); this.AddedDistance = robot.configuration.variable(groupName, actionName, "AddedDistance").value(); + this.directPosition = robot.configuration.variable(groupName, actionName, "directPosition").value(); this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; @@ -35,22 +37,31 @@ public class BottomArm extends CyberarmState { return; } - if (System.currentTimeMillis() - lastStepTime >= time) { - lastStepTime = System.currentTimeMillis(); + if (directPosition) { + robot.LowRiserLeft.setPosition(LowerRiserLeftPos); + robot.LowRiserRight.setPosition(LowerRiserLeftPos); - if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) { - - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance); - - } else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) { - - robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance); - robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance); - - } else { + if (runTime() >= time){ setHasFinished(true); } + } else { + if (System.currentTimeMillis() - lastStepTime >= time) { + lastStepTime = System.currentTimeMillis(); + + if (robot.LowRiserLeft.getPosition() < LowerRiserLeftPos && up) { + + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() + AddedDistance); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() + AddedDistance); + + } else if (robot.LowRiserLeft.getPosition() > LowerRiserLeftPos && !up) { + + robot.LowRiserLeft.setPosition(robot.LowRiserLeft.getPosition() - AddedDistance); + robot.LowRiserRight.setPosition(robot.LowRiserRight.getPosition() - AddedDistance); + + } else { + setHasFinished(true); + } + } } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java index 3e8abe6..b6982e4 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorDistanceState.java @@ -9,6 +9,56 @@ import org.timecrafters.testing.states.PrototypeBot1; public class CollectorDistanceState extends CyberarmState { private final PrototypeBot1 robot; + private int traveledDistance; + private int RampUpDistance; + private int RampDownDistance; + private double drivePower; + private double targetDrivePower; + private double lastMeasuredTime; + private double currentDistance; + private double LastDistanceRead; + private double distanceDelta; + private double debugRunTime; + private String debugStatus = "?"; + + + public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); + this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value(); + this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value(); + this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value(); + + } + + @Override + public void telemetry() { + engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition()); + engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition()); + engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition()); + engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition()); + engine.telemetry.addLine(); + + engine.telemetry.addData("traveledDistance", traveledDistance); + engine.telemetry.addData("RampUpDistance", RampUpDistance); + engine.telemetry.addData("RampDownDistance", RampDownDistance); + + engine.telemetry.addLine(); + + engine.telemetry.addData("drivePower", drivePower); + engine.telemetry.addData("targetDrivePower", targetDrivePower); + + engine.telemetry.addLine(); + engine.telemetry.addData("Distance Sensor", robot.collectorDistance.getDistance(DistanceUnit.MM)); + engine.telemetry.addData("Current Distance", currentDistance); + engine.telemetry.addData("last Distance", LastDistanceRead); + engine.telemetry.addLine(); + + engine.telemetry.addData("distanceDelta", distanceDelta); + engine.telemetry.addData("DEBUG RunTime", debugRunTime); + engine.telemetry.addData("DEBUG Status", debugStatus); + + } @Override public void start() { @@ -26,17 +76,72 @@ public class CollectorDistanceState extends CyberarmState { robot.collectorLeft.setPower(1); robot.collectorRight.setPower(1); + + lastMeasuredTime = System.currentTimeMillis(); + LastDistanceRead = robot.collectorDistance.getDistance(DistanceUnit.MM); + + + } @Override public void exec() { - if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) { - robot.frontRightDrive.setPower(0.15); - robot.frontLeftDrive.setPower(0.15); - robot.backRightDrive.setPower(0.15); - robot.backLeftDrive.setPower(0.15); + + if (System.currentTimeMillis() - lastMeasuredTime > 150) { + // checking to see if time is greater than 150 milliseconds + lastMeasuredTime = System.currentTimeMillis(); + + currentDistance = robot.collectorDistance.getDistance(DistanceUnit.MM); + + distanceDelta = LastDistanceRead - currentDistance; + + if (distanceDelta >= 0.0 || currentDistance > 500) { + // I am moving forward + // and im close too my target. + LastDistanceRead = currentDistance; + debugRunTime = runTime(); + debugStatus = "Driving Towards Cone"; + } else { + // I have stopped + debugStatus = "Nothing Collected"; + robot.collectorLeft.setPower(0); + robot.collectorRight.setPower(0); + setHasFinished(true); + return; + } + } + + if (robot.collectorDistance.getDistance(DistanceUnit.MM) > 60) { + double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); + + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) { + // ramping up + drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.15; + } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) { + // ramping down + drivePower = ((delta / RampDownDistance) + 0.15); + } else { + // middle ground + drivePower = targetDrivePower; + } + + if (Math.abs(drivePower) > Math.abs(targetDrivePower)) { + // This is limiting drive power to the targeted drive power + drivePower = targetDrivePower; + } + + if (targetDrivePower < 0 && drivePower != targetDrivePower) { + drivePower = drivePower * -1; + } + + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) { + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower); + } } else { robot.frontRightDrive.setPower(0); robot.frontLeftDrive.setPower(0); @@ -46,12 +151,9 @@ public class CollectorDistanceState extends CyberarmState { robot.collectorLeft.setPower(0); setHasFinished(true); - } + } + + } } - public CollectorDistanceState(PrototypeBot1 robot, String groupName, String actionName) { - this.robot = robot; -} - -} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java new file mode 100644 index 0000000..4255cb7 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/ConeIdentification.java @@ -0,0 +1,98 @@ +package org.timecrafters.Autonomous.States; + +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry; + +import org.cyberarm.engine.V2.CyberarmState; +import org.firstinspires.ftc.robotcore.external.tfod.Recognition; +import org.timecrafters.testing.states.PrototypeBot1; + +import java.util.List; + +public class ConeIdentification extends CyberarmState { + PrototypeBot1 robot; + private int time; + private float minimumConfidence; + private int ParkPlace; + + public ConeIdentification(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; + minimumConfidence = robot.configuration.variable(groupName, actionName, "Minimum Confidence").value(); + time = robot.configuration.variable(groupName, actionName, "time").value(); + } + + @Override + public void init() { + engine.blackboard.put("parkPlace", "1"); + robot.tfod.activate(); + } + + @Override + public void telemetry() { + if (robot.tfod != null) { + // getUpdatedRecognitions() will return null if no new information is available since + // the last time that call was made. + List updatedRecognitions = robot.tfod.getUpdatedRecognitions(); + if (updatedRecognitions != null) { + engine.telemetry.addData("# Objects Detected", updatedRecognitions.size()); + + // step through the list of recognitions and display image position/size information for each one + // Note: "Image number" refers to the randomized image orientation/number + for (Recognition recognition : updatedRecognitions) { + double col = (recognition.getLeft() + recognition.getRight()) / 2 ; + double row = (recognition.getTop() + recognition.getBottom()) / 2 ; + double width = Math.abs(recognition.getRight() - recognition.getLeft()) ; + double height = Math.abs(recognition.getTop() - recognition.getBottom()) ; + + engine.telemetry.addData("Label", recognition.getLabel()); + engine.telemetry.addData(""," "); + engine.telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100 ); + engine.telemetry.addData("- Position (Row/Col)","%.0f / %.0f", row, col); + engine.telemetry.addData("- Size (Width/Height)","%.0f / %.0f", width, height); + + if (recognition.getLabel().equals("2 Bulb")) { + engine.telemetry.addData("2 Bulb", engine.blackboard.put("parkPlace", "2")); + } else if (recognition.getLabel().equals("3 Panel")) { + engine.telemetry.addData("3 Panel",engine.blackboard.put("parkPlace", "3")); + } else { + engine.telemetry.addData("1 Bolt", engine.blackboard.put("parkPlace", "1")); + } + } + } + } + } + + @Override + public void exec() { + + if (robot.tfod != null) { + // getUpdatedRecognitions() will return null if no new information is available since + // the last time that call was made. + List updatedRecognitions = robot.tfod.getUpdatedRecognitions(); + if (updatedRecognitions != null) { + // step through the list of recognitions and display image position/size information for each one + // Note: "Image number" refers to the randomized image orientation/number + float bestConfidence = 0; + + for (Recognition recognition : updatedRecognitions) { + if (recognition.getConfidence() >= minimumConfidence && recognition.getConfidence() > bestConfidence) { + bestConfidence = recognition.getConfidence(); + + if (recognition.getLabel().equals("2 Bulb")) { + engine.blackboard.put("parkPlace", "2"); + } else if (recognition.getLabel().equals("3 Panel")) { + engine.blackboard.put("parkPlace", "3"); + + } else { + engine.blackboard.put("parkPlace", "1"); + } + } + } + } + } + + if (runTime() >= time){ + setHasFinished(true); + } + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java new file mode 100644 index 0000000..5b918a3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverParkPlaceState.java @@ -0,0 +1,108 @@ +package org.timecrafters.Autonomous.States; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.testing.states.PrototypeBot1; + +public class DriverParkPlaceState extends CyberarmState { + private final boolean stateDisabled; + PrototypeBot1 robot; + private int RampUpDistance; + private int RampDownDistance; + private String intendedPlacement; + + public DriverParkPlaceState(PrototypeBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); + this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value(); + this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value(); + this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value(); + this.intendedPlacement = robot.configuration.variable(groupName, actionName, "intendedPlacement").value(); + this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; + } + private double drivePower, targetDrivePower; + private int traveledDistance; + + @Override + public void start() { + robot.frontRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.frontLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.backRightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + robot.backLeftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + robot.frontRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.frontLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.backRightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.backLeftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + + @Override + public void exec() { + if (stateDisabled) { + setHasFinished(true); + return; + } + String placement = engine.blackboard.get("parkPlace"); + if (placement != null) { + if (!placement.equals(intendedPlacement)){ + setHasFinished(true); + } + if (placement.equals(intendedPlacement)) { + double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); + + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance) { + // ramping up + drivePower = (Math.abs((float) robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25; + } else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta) { + // ramping down + drivePower = ((delta / RampDownDistance) + 0.25); + } else { + // middle ground + drivePower = targetDrivePower; + } + + if (Math.abs(drivePower) > Math.abs(targetDrivePower)) { + // This is limiting drive power to the targeted drive power + drivePower = targetDrivePower; + } + + if (targetDrivePower < 0 && drivePower != targetDrivePower) { + drivePower = drivePower * -1; + } + + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance) { + robot.backLeftDrive.setPower(drivePower); + robot.backRightDrive.setPower(drivePower); + robot.frontLeftDrive.setPower(drivePower); + robot.frontRightDrive.setPower(drivePower); + } else { + robot.backLeftDrive.setPower(0); + robot.backRightDrive.setPower(0); + robot.frontLeftDrive.setPower(0); + robot.frontRightDrive.setPower(0); + setHasFinished(true); + } // else ending + } // placement equals if statement +// setHasFinished(true); + } // end of placement doesn't equal null + + } // end of exec + + @Override + public void telemetry() { + engine.telemetry.addData("Position", intendedPlacement); + engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition()); + engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition()); + engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition()); + engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition()); + + engine.telemetry.addData("drivePower", drivePower); + engine.telemetry.addData("targetDrivePower", targetDrivePower); + + engine.telemetry.addData("traveledDistance", traveledDistance); + engine.telemetry.addData("RampUpDistance", RampUpDistance); + engine.telemetry.addData("RampDownDistance", RampDownDistance); + + } // end of telemetry +} // end of class diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java index 22ebba6..e48552f 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverState.java @@ -8,14 +8,18 @@ import org.timecrafters.testing.states.PrototypeBot1; public class DriverState extends CyberarmState { private final boolean stateDisabled; PrototypeBot1 robot; + private int RampUpDistance; + private int RampDownDistance; public DriverState(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; - this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value(); + this.targetDrivePower = robot.configuration.variable(groupName, actionName, "targetDrivePower").value(); this.traveledDistance = robot.configuration.variable(groupName, actionName, "traveledDistance").value(); + this.RampUpDistance = robot.configuration.variable(groupName, actionName, "RampUpDistance").value(); + this.RampDownDistance = robot.configuration.variable(groupName, actionName, "RampDownDistance").value(); this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; } - private double drivePower; + private double drivePower, targetDrivePower; private int traveledDistance; @Override @@ -38,6 +42,29 @@ public class DriverState extends CyberarmState { return; } + double delta = traveledDistance - Math.abs(robot.frontRightDrive.getCurrentPosition()); + + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) <= RampUpDistance){ + // ramping up + drivePower = (Math.abs((float)robot.frontRightDrive.getCurrentPosition()) / RampUpDistance) + 0.25; + } + else if (Math.abs(robot.frontRightDrive.getCurrentPosition()) >= delta){ + // ramping down + drivePower = ((delta / RampDownDistance) + 0.25); + } else { + // middle ground + drivePower = targetDrivePower; + } + + if (Math.abs(drivePower) > Math.abs(targetDrivePower)){ + // This is limiting drive power to the targeted drive power + drivePower = targetDrivePower; + } + + if (targetDrivePower < 0 && drivePower != targetDrivePower) { + drivePower = drivePower * -1; + } + if (Math.abs(robot.frontRightDrive.getCurrentPosition()) < traveledDistance){ robot.backLeftDrive.setPower(drivePower); robot.backRightDrive.setPower(drivePower); @@ -51,5 +78,22 @@ public class DriverState extends CyberarmState { setHasFinished(true); } + + } + + @Override + public void telemetry() { + engine.telemetry.addData("frontRightDrive", robot.frontRightDrive.getCurrentPosition()); + engine.telemetry.addData("frontLeftDrive", robot.frontLeftDrive.getCurrentPosition()); + engine.telemetry.addData("BackRightDrive", robot.backRightDrive.getCurrentPosition()); + engine.telemetry.addData("BackLeftDrive", robot.backLeftDrive.getCurrentPosition()); + + engine.telemetry.addData("drivePower", drivePower); + engine.telemetry.addData("targetDrivePower", targetDrivePower); + + engine.telemetry.addData("traveledDistance", traveledDistance); + engine.telemetry.addData("RampUpDistance", RampUpDistance); + engine.telemetry.addData("RampDownDistance", RampDownDistance); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java new file mode 100644 index 0000000..672b240 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/PathDecision.java @@ -0,0 +1,29 @@ +package org.timecrafters.Autonomous.States; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.testing.states.PrototypeBot1; + +public class PathDecision extends CyberarmState { + PrototypeBot1 robot; + private String groupName; + + + public PathDecision (PrototypeBot1 robot, String groupName, String actionName){ + this.robot = robot; + this.groupName = groupName; + + } + + @Override + public void exec() { + String placement = engine.blackboard.get("parkPlace"); + setHasFinished(true); + } + + } + + + + + + diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java index 4cd2124..defdcc6 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -8,8 +8,9 @@ public class RotationState extends CyberarmState { PrototypeBot1 robot; public RotationState(PrototypeBot1 robot, String groupName, String actionName) { this.robot = robot; - this.drivePower = robot.configuration.variable(groupName, actionName, "drivePower").value(); + this.drivePower = robot.configuration.variable(groupName, actionName, "DrivePower").value(); this.targetRotation = robot.configuration.variable(groupName, actionName, "targetRotation").value(); + drivePowerVariable = drivePower; this.stateDisabled = !robot.configuration.action(groupName, actionName).enabled; @@ -18,20 +19,41 @@ public class RotationState extends CyberarmState { private double drivePower; private float targetRotation; float RobotRotation; + private double RotationTarget; + private double RotationDirectionMinimum; + private String debugStatus = "?"; + private double drivePowerVariable; + @Override public void exec() { if (stateDisabled){ setHasFinished(true); return; - } + } // end of if RobotRotation = robot.imu.getAngularOrientation().firstAngle; - if (RobotRotation <= targetRotation -3 || RobotRotation >= targetRotation + 3) { - robot.backLeftDrive.setPower(-drivePower); - robot.backRightDrive.setPower(drivePower); - robot.frontLeftDrive.setPower(-drivePower); - robot.frontRightDrive.setPower(drivePower); + + if (Math.abs(Math.abs(targetRotation) - Math.abs(RobotRotation)) < 20){ + drivePowerVariable = 0.3 * drivePower; + debugStatus = "Rotate Slow"; + } // end of if + else { + drivePowerVariable = drivePower * 0.75; + debugStatus = "Rotate Fast"; + } + + if (RobotRotation >= targetRotation + 1){ + drivePowerVariable = Math.abs(drivePowerVariable); + } else { + drivePowerVariable = -1 * Math.abs(drivePowerVariable); + } + + if (RobotRotation <= targetRotation -1 || RobotRotation >= targetRotation + 1) { + robot.backLeftDrive.setPower(drivePowerVariable); + robot.backRightDrive.setPower(-drivePowerVariable); + robot.frontLeftDrive.setPower(drivePowerVariable); + robot.frontRightDrive.setPower(-drivePowerVariable); } else { robot.backLeftDrive.setPower(0); @@ -45,9 +67,13 @@ public class RotationState extends CyberarmState { @Override public void telemetry() { + engine.telemetry.addData("DEBUG Status", debugStatus); + + engine.telemetry.addLine(); + engine.telemetry.addData("Robot IMU Rotation", RobotRotation); engine.telemetry.addData("Robot Target Rotation", targetRotation); - engine.telemetry.addData("Drive Power", drivePower); + engine.telemetry.addData("Drive Power", drivePowerVariable); } } diff --git a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java index 99890ae..3d46479 100644 --- a/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java +++ b/TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/states/MecanumMinibotTeleOpState.java @@ -71,7 +71,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState { } /* ............................................................................ grab */ - if(engine.gamepad1.left_stick_x>0.5 || engine.gamepad1.right_stick_x<-0.5 || + if(engine.gamepad1.left_stick_x < -0.5 || engine.gamepad1.right_stick_x < -0.5 || engine.gamepad2.x){ // in robot.pServoGrab.setPosition(0.9); } @@ -79,7 +79,7 @@ public class MecanumMinibotTeleOpState extends CyberarmState { engine.gamepad2.back){ // small out robot.pServoGrab.setPosition(0.50); } - if(engine.gamepad1.right_stick_x>0.5 || engine.gamepad1.right_stick_x>0.5 || + if(engine.gamepad1.left_stick_x > 0.5 || engine.gamepad1.right_stick_x > 0.5 || engine.gamepad2.b){ // big out robot.pServoGrab.setPosition(0.0); } diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java index 4b7a5e1..24724ff 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeBot1.java @@ -29,11 +29,11 @@ public class PrototypeBot1 { private static final String VUFORIA_KEY = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah"; - private VuforiaLocalizer vuforia; + public VuforiaLocalizer vuforia; - private TFObjectDetector tfod; + public TFObjectDetector tfod; - public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight; + public Servo LowRiserLeft, LowRiserRight, HighRiserLeft, HighRiserRight, CameraServo; private final CyberarmEngine engine; public Rev2mDistanceSensor collectorDistance; @@ -80,6 +80,9 @@ public class PrototypeBot1 { // servo configuration + //Camera Servo + CameraServo = engine.hardwareMap.servo.get("Camera Servo"); + // Collector collectorLeft = engine.hardwareMap.crservo.get("Collector Left"); collectorRight = engine.hardwareMap.crservo.get("Collector Right"); @@ -117,22 +120,26 @@ public class PrototypeBot1 { LowRiserLeft.setDirection(Servo.Direction.FORWARD); LowRiserRight.setDirection(Servo.Direction.REVERSE); - LowRiserLeft.setPosition(0.45); - LowRiserRight.setPosition(0.45); + LowRiserLeft.setPosition(0.35); + LowRiserRight.setPosition(0.35); HighRiserLeft.setPosition(0.45); HighRiserRight.setPosition(0.45); + CameraServo.setPosition(0); + } private void initVuforia(){ /* * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine. */ - VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(); + int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName()); + VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId); parameters.vuforiaLicenseKey = VUFORIA_KEY; parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1"); + // Instantiate the Vuforia engine vuforia = ClassFactory.getInstance().createVuforia(parameters); } diff --git a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java index 855c01c..851e9e7 100644 --- a/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java +++ b/TeamCode/src/main/java/org/timecrafters/testing/states/PrototypeTeleOPState.java @@ -290,7 +290,7 @@ public class PrototypeTeleOPState extends CyberarmState { } if (engine.gamepad2.dpad_up) { - if (robot.HighRiserLeft.getPosition() < 0.9) { + if (robot.HighRiserLeft.getPosition() < 1.0) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05); @@ -310,7 +310,7 @@ public class PrototypeTeleOPState extends CyberarmState { } if (engine.gamepad2.y) { - if (robot.HighRiserLeft.getPosition() < 0.84) { + if (robot.HighRiserLeft.getPosition() < 0.9) { if (System.currentTimeMillis() - lastStepTime >= 150) { lastStepTime = System.currentTimeMillis(); robot.HighRiserLeft.setPosition(robot.HighRiserLeft.getPosition() + 0.05);