From f2bd08a69a352c74c8aa3716a4e778567ce966c9 Mon Sep 17 00:00:00 2001 From: Sodi Date: Mon, 6 Feb 2023 20:23:13 -0600 Subject: [PATCH] Autonomous config --- .../Engines/LeftTwoConeAutonomousEngine.java | 2 +- .../Engines/RightStateAutoEngine.java | 17 +++++---- .../Engines/RightTwoConeAutonomousEngine.java | 2 +- .../Autonomous/States/CollectorState.java | 4 +-- .../States/DriverStateWithOdometer.java | 2 +- .../Autonomous/States/RotationState.java | 2 +- .../Autonomous/States/TopArmv2.java | 36 +++++++++++++++++++ 7 files changed, 52 insertions(+), 13 deletions(-) create mode 100644 TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmv2.java diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java index 0307501..b67b6e7 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/LeftTwoConeAutonomousEngine.java @@ -17,7 +17,7 @@ import org.timecrafters.Autonomous.States.ServoCameraRotate; import org.timecrafters.Autonomous.States.TopArm; import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; -@Autonomous (name = "left 2 cone auto") +//@Autonomous (name = "left 2 cone auto") public class LeftTwoConeAutonomousEngine extends CyberarmEngine { PhoenixBot1 robot; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java index c29d2c8..d68b0e9 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightStateAutoEngine.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import org.cyberarm.engine.V2.CyberarmEngine; import org.timecrafters.Autonomous.States.DriverStateWithOdometer; import org.timecrafters.Autonomous.States.RotationState; +import org.timecrafters.Autonomous.States.TopArmv2; import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; @Autonomous(name = "Right Side") @@ -16,15 +17,17 @@ public class RightStateAutoEngine extends CyberarmEngine { robot = new PhoenixBot1(this); robot.imu.resetYaw(); -// setupFromConfig( -// robot.configuration, -// "org.timecrafters.Autonomous.States", -// robot, -// PhoenixBot1.class, -// "Right State Auto"); + setupFromConfig( + robot.configuration, + "org.timecrafters.Autonomous.States", + robot, + PhoenixBot1.class, + "Right State Auto"); + +// addState(new TopArmv2(robot, "Right State Auto", "06-0")); // // driving forward to low junction - addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0")); +// addState(new DriverStateWithOdometer(robot, "Right State Auto", "02-0")); // // rotate left towards low junction // addState(new RotationState(robot, "Right State Auto", "02-1")); // // driving forward/ backwards to adjust diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java index 8ed808c..ae94713 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/Engines/RightTwoConeAutonomousEngine.java @@ -17,7 +17,7 @@ import org.timecrafters.Autonomous.States.ServoCameraRotate; import org.timecrafters.Autonomous.States.TopArm; import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; -@Autonomous (name = "Right 2 cone auto") +//@Autonomous (name = "Right 2 cone auto") public class RightTwoConeAutonomousEngine extends CyberarmEngine { PhoenixBot1 robot; diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java index 36f88b9..e8b67b6 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/CollectorState.java @@ -52,8 +52,8 @@ public class CollectorState extends CyberarmState { } } else { -// robot.collectorLeft.setPower(0); -// robot.collectorRight.setPower(0); + robot.collectorLeft.setPower(0); + robot.collectorRight.setPower(0); setHasFinished(true); } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java index 08a5c47..22d2911 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/DriverStateWithOdometer.java @@ -287,7 +287,7 @@ public class DriverStateWithOdometer extends CyberarmState { } if (driveStage == 5) { -// setHasFinished(true); + 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 cb1ae2b..1e09d22 100644 --- a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/RotationState.java @@ -104,7 +104,7 @@ public class RotationState extends CyberarmState { robot.backLeftDrive.setPower(0); RotationStage ++; -// setHasFinished(true); + setHasFinished(true); } } diff --git a/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmv2.java b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmv2.java new file mode 100644 index 0000000..043a9a3 --- /dev/null +++ b/TeamCode/src/main/java/org/timecrafters/Autonomous/States/TopArmv2.java @@ -0,0 +1,36 @@ +package org.timecrafters.Autonomous.States; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.cyberarm.engine.V2.CyberarmState; +import org.timecrafters.Autonomous.TeleOp.states.PhoenixBot1; + +public class TopArmv2 extends CyberarmState { + private final PhoenixBot1 robot; + private double targetPower; + private int targetPosition; + private int tolerance; + + + public TopArmv2(PhoenixBot1 robot, String groupName, String actionName) { + this.robot = robot; + this.targetPosition = robot.configuration.variable(groupName, actionName, "targetPosition").value(); + this.targetPower = robot.configuration.variable(groupName, actionName, "targetPower").value(); + this.tolerance = robot.configuration.variable(groupName, actionName, "tolerance").value(); + } + + @Override + public void start() { + robot.ArmMotor.setTargetPosition(targetPosition); + robot.ArmMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION); + robot.ArmMotor.setPower(targetPower); + } + + @Override + public void exec() { + + if (robot.ArmMotor.getTargetPosition() == targetPosition) { + setHasFinished(true); + } + } +}