diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueDuckEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueDuckEngine.java new file mode 100644 index 0000000..0287337 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueDuckEngine.java @@ -0,0 +1,59 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.CollectorToggle; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DriveState; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TensorFlowState; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmExtension; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretOrbit; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; + +@Autonomous (name = "Blue Duck Autonomous", group = "blue") +public class BlueDuckEngine extends CyberarmEngine { + Robot robot; + + @Override + public void setup() { + this.robot = new Robot(this); + robot.resetEncoders(); + + addState(new TurretOrbit(robot, robot.turretServoWhite, robot.whiteMag, "BlueDuckAutonomous", "01_0")); + addState(new TensorFlowState(robot, robot.whiteArmRiser, robot.whiteArmBobbin, "BlueDuckAutonomous", "01_1")); +// addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "02_0")); +// addState(new TurretArmRiser(robot, robot.whiteArmRiser, "RedWarehouseAutonomous", "03_0_middle")); +// addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "RedWarehouseAutonomous", "04_0")); + addState(new CollectorToggle(robot, robot.collectorWhite, "BlueDuckAutonomous", "05_0")); + addState(new CollectorToggle(robot, robot.collectorWhite, "BlueDuckAutonomous", "06_0")); + addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "BlueDuckAutonomous", "07_0")); + addState(new DriveState(robot,"BlueDuckAutonomous", "08_0")); + addState(new DriveState(robot, "BlueDuckAutonomous", "09_0")); + addState(new TurretOrbit(robot, robot.turretServoWhite, null, "BlueDuckAutonomous", "10_0")); + addState(new DriveState(robot, "BlueDuckAutonomous", "10_1")); + + } + + public void loop() { + super.loop(); + + telemetry.addData("white arm extension", robot.whiteArmBobbin.getCurrentPosition()); + telemetry.addData("White Riser Arm", robot.whiteArmRiser.getCurrentPosition()); + telemetry.addData("white Turret Switch", robot.whiteMag.isPressed()); + telemetry.addData("white Turret orbit Power", robot.turretServoWhite.getPower()); + telemetry.addData("White Door Position", robot.whiteDispenser.getPosition()); + telemetry.addLine(); + + telemetry.addData("Orange Riser Arm", robot.orangeArmRiser.getCurrentPosition()); + telemetry.addData("orange arm extension", robot.orangeArmBobbin.getCurrentPosition()); + telemetry.addData("orange Turret Switch", robot.orangeMag.isPressed()); + telemetry.addData("orange Turret Orbit Power", robot.turretServoOrange.getPower()); + telemetry.addData("Orange Door Position", robot.orangeDispenser.getPosition()); + telemetry.addLine(); + + telemetry.addData("driveWarehouseLeft", robot.driveWarehouseLeft.getCurrentPosition()); + telemetry.addData("driveWarehouseRight", robot.driveWarehouseRight.getCurrentPosition()); + telemetry.addData("driveGoalLeft", robot.driveGoalLeft.getCurrentPosition()); + telemetry.addData("driveGoalRight", robot.driveGoalRight.getCurrentPosition()); + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueWarehouseEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueWarehouseEngine.java index 7a1603e..2f9d892 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueWarehouseEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/BlueWarehouseEngine.java @@ -18,18 +18,20 @@ public class BlueWarehouseEngine extends CyberarmEngine { @Override public void setup() { this.robot = new Robot(this); + robot.resetEncoders(); - addState(new TurretOrbit(robot, robot.turretServoWhite, robot.whiteMag, "BlueWarehouseAutonomous", "01_0")); - addState(new TensorFlowState(robot, robot.whiteArmRiser, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "01_1")); -// addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "02_0")); -// addState(new TurretArmRiser(robot, robot.whiteArmRiser, "BlueWarehouseAutonomous", "03_0_middle")); -// addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "04_0_middle")); - addState(new CollectorToggle(robot, robot.collectorWhite, "BlueWarehouseAutonomous", "05_0")); - addState(new CollectorToggle(robot, robot.collectorWhite, "BlueWarehouseAutonomous", "06_0")); - addState(new TurretArmExtension(robot, robot.whiteArmBobbin, "BlueWarehouseAutonomous", "07_0")); + addState(new TurretOrbit(robot, robot.turretServoOrange, robot.orangeMag, "BlueWarehouseAutonomous", "01_0")); + addState(new TensorFlowState(robot, robot.orangeArmRiser, robot.orangeArmBobbin, "BlueWarehouseAutonomous", "01_1")); +// addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "RedWarehouseAutonomous", "02_0")); +// addState(new TurretArmRiser(robot, robot.orangeArmRiser, "RedWarehouseAutonomous", "03_0_middle")); +// addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "RedWarehouseAutonomous", "04_0")); + addState(new CollectorToggle(robot, robot.collectorOrange, "BlueWarehouseAutonomous", "05_0")); + addState(new CollectorToggle(robot, robot.collectorOrange, "BlueWarehouseAutonomous", "06_0")); + addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "BlueWarehouseAutonomous", "07_0")); addState(new DriveState(robot,"BlueWarehouseAutonomous", "08_0")); addState(new DriveState(robot, "BlueWarehouseAutonomous", "09_0")); - addState(new DriveState(robot, "BlueWarehouseAutonomous", "10_0")); + addState(new TurretOrbit(robot, robot.turretServoOrange, null, "BlueWarehouseAutonomous", "10_0")); + addState(new DriveState(robot, "BlueWarehouseAutonomous", "10_1")); } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedDuckEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedDuckEngine.java new file mode 100644 index 0000000..2bcc8a7 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedDuckEngine.java @@ -0,0 +1,36 @@ +package org.timecrafters.FreightFrenzy.Competition.Autonomous.Engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.cyberarm.engine.V2.CyberarmEngine; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.CollectorToggle; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.DriveState; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TensorFlowState; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretArmExtension; +import org.timecrafters.FreightFrenzy.Competition.Autonomous.States.TurretOrbit; +import org.timecrafters.FreightFrenzy.Competition.Common.Robot; + +@Autonomous(name = "RedDuckAutonomous", group = "red") + +public class RedDuckEngine extends CyberarmEngine { + + @Override + public void setup() { + Robot robot = new Robot(this); + robot.resetEncoders(); + + addState(new TurretOrbit(robot, robot.turretServoOrange, robot.orangeMag, "RedDuckAutonomous", "01_0")); + addState(new TensorFlowState(robot, robot.orangeArmRiser, robot.orangeArmBobbin, "RedDuckAutonomous", "01_1")); +// addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "RedWarehouseAutonomous", "02_0")); +// addState(new TurretArmRiser(robot, robot.orangeArmRiser, "RedWarehouseAutonomous", "03_0_middle")); +// addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "RedWarehouseAutonomous", "04_0")); + addState(new CollectorToggle(robot, robot.collectorOrange, "RedDuckAutonomous", "05_0")); + addState(new CollectorToggle(robot, robot.collectorOrange, "RedDuckAutonomous", "06_0")); + addState(new TurretArmExtension(robot, robot.orangeArmBobbin, "RedDuckAutonomous", "07_0")); + addState(new DriveState(robot,"RedDuckAutonomous", "08_0")); + addState(new DriveState(robot, "RedDuckAutonomous", "09_0")); + addState(new TurretOrbit(robot, robot.turretServoOrange, null, "RedDuckAutonomous", "10_0")); + addState(new DriveState(robot, "RedDuckAutonomous", "10_1")); + + } +} diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java index 81cbfc9..b69ea91 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/Engines/RedWarehouseEngine.java @@ -18,6 +18,7 @@ public class RedWarehouseEngine extends CyberarmEngine { @Override public void setup() { this.robot = new Robot(this); + robot.resetEncoders(); addState(new TurretOrbit(robot, robot.turretServoWhite, robot.whiteMag, "RedWarehouseAutonomous", "01_0")); addState(new TensorFlowState(robot, robot.whiteArmRiser, robot.whiteArmBobbin, "RedWarehouseAutonomous", "01_1")); diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java index 27d2277..af12250 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/CollectorToggle.java @@ -12,7 +12,7 @@ public class CollectorToggle extends CyberarmState { final static public int MODE_STOPPED = 0; double time; CRServo servo; - int power; + double power; public CollectorToggle(Robot robot, CRServo servo, String groupName, String actionName) { this.servo = servo; diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TensorFlowState.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TensorFlowState.java index eb1c3ff..74fa26c 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TensorFlowState.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TensorFlowState.java @@ -15,6 +15,8 @@ public class TensorFlowState extends CyberarmState { private double checkTime; private int manualPath; private int path = 0; + private double leftDuck; + private double middleDuck; private String groupName; private DcMotor armRiser, armExtension; @@ -24,6 +26,8 @@ public class TensorFlowState extends CyberarmState { this.groupName = groupName; this.armRiser = armRiser; this.armExtension = armExtension; + this.leftDuck = robot.configuration.variable(groupName, actionName, "leftDuck").value(); + this.middleDuck = robot.configuration.variable(groupName, actionName, "middleDuck").value(); } @Override @@ -35,7 +39,7 @@ public class TensorFlowState extends CyberarmState { @Override public void exec() { recognitions = robot.tensorflowDetections(); -// + if (runTime() < checkTime) { if (manualPath != -1) { @@ -43,12 +47,10 @@ public class TensorFlowState extends CyberarmState { if (recognitions.size() == 1) { Recognition recognition = recognitions.get(0); - if (recognition.getLabel().equals("duck")){ - if (recognition.getLeft() < 320) { - path = 0; - } else { - path = 1; - } + if (recognition.getLeft() < leftDuck) { + path = 0; + } else { + path = 1; } } else { path = 2; @@ -70,7 +72,7 @@ public class TensorFlowState extends CyberarmState { addState(new TurretArmExtension(robot, armExtension, groupName, "04_0_top")); } - setHasFinished(true); + setHasFinished(true); } } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java index a636e47..021b792 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmExtension.java @@ -20,12 +20,6 @@ public class TurretArmExtension extends CyberarmState { this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value(); } - @Override - public void start() { - motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - } - @Override public void exec() { if (motor.getCurrentPosition() < targetPosition - tolerance){ @@ -40,4 +34,9 @@ public class TurretArmExtension extends CyberarmState { motor.setPower(0); } } + + @Override + public void telemetry() { + engine.telemetry.addData("targetPosition", targetPosition); + } } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java index f397eb1..177a764 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretArmRiser.java @@ -20,12 +20,6 @@ public class TurretArmRiser extends CyberarmState { this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value(); } - @Override - public void start() { - motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - motor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - } - @Override public void exec() { if (motor.getCurrentPosition() < targetPosition - tolerance){ diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java index 7f1611d..f863c1b 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Autonomous/States/TurretOrbit.java @@ -24,7 +24,7 @@ public class TurretOrbit extends CyberarmState { @Override public void exec() { - if (magnetSwitch.isPressed() || runTime() < time ){ + if ((magnetSwitch != null && magnetSwitch.isPressed()) || runTime() > time ){ servo.setPower(0); setHasFinished(true); } diff --git a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java index 15166d5..b1d93a3 100644 --- a/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java +++ b/TeamCode/src/main/java/org/timecrafters/FreightFrenzy/Competition/Common/Robot.java @@ -274,6 +274,18 @@ public class Robot { } } + public void resetEncoders(){ + orangeArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + whiteArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + orangeArmRiser.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + whiteArmRiser.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + orangeArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + whiteArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + orangeArmRiser.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + whiteArmRiser.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + private void initTensorflow() { int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier( "tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());