From b84619bfc58c4d8b213ba9119466c6f628cf525a Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Mon, 11 Dec 2023 09:20:13 -0600 Subject: [PATCH 1/5] WIP: Experimental support for live reloading TCT Config variables --- .../dev/cyberarm/engine/V2/CyberarmState.java | 7 +++++ .../library/TimeCraftersConfiguration.java | 25 +++++++++++++++++ .../library/backend/Config.java | 28 +++++++++++++++++++ 3 files changed, 60 insertions(+) diff --git a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmState.java b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmState.java index d0a867c..ed83fdc 100644 --- a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmState.java +++ b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmState.java @@ -172,6 +172,13 @@ public abstract class CyberarmState implements Runnable { return hasFinished; } + /** + * Sets state as finished + */ + public void finished() { + hasFinished = true; + } + /** * * @return Get value of isRunning diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/TimeCraftersConfiguration.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/TimeCraftersConfiguration.java index 5c52a36..ed81d7f 100644 --- a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/TimeCraftersConfiguration.java +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/TimeCraftersConfiguration.java @@ -1,5 +1,9 @@ package org.timecrafters.TimeCraftersConfigurationTool.library; +import android.os.FileObserver; + +import androidx.annotation.Nullable; + import com.google.gson.Gson; import com.google.gson.GsonBuilder; @@ -33,6 +37,8 @@ import java.io.FileReader; public class TimeCraftersConfiguration { private static final String TAG = "TCT|TCConfig"; private Config config; + private boolean liveReload = false; + private FileObserver fileObserver; public TimeCraftersConfiguration() { Settings settings = loadSettings(); @@ -47,6 +53,10 @@ public class TimeCraftersConfiguration { return config; } + public void enableLiveReload(boolean b) { + liveReload = b; + } + public Group group(String groupName) { for (final Group group : config.getGroups()) { if (group.name.trim().equals(groupName.trim())) { @@ -109,6 +119,21 @@ public class TimeCraftersConfiguration { Config config = gsonForConfig().fromJson(new FileReader(configFile), Config.class); config.setName(name); + final TimeCraftersConfiguration self = this; + if (fileObserver == null) { + fileObserver = new FileObserver(configFile) { + @Override + public void onEvent(int event, @Nullable String filePath) { + if (self.liveReload && event == CLOSE_WRITE) { + Config newConfig = loadConfig(path); + + self.config.syncVariables(newConfig); + } + } + }; + fileObserver.startWatching(); + } + return config; } catch (FileNotFoundException e) { e.printStackTrace(); diff --git a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Config.java b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Config.java index 8054c72..edfef13 100644 --- a/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Config.java +++ b/TeamCode/src/main/java/org/timecrafters/TimeCraftersConfigurationTool/library/backend/Config.java @@ -4,6 +4,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Act import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Configuration; import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Group; import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Presets; +import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; import java.util.ArrayList; import java.util.Date; @@ -41,4 +42,31 @@ public class Config { public ArrayList getGroups() { return groups; } + + /** + * Syncs the values of Variables from reloaded config + * @param newConfig + */ + public void syncVariables(Config newConfig) { + for (Group group : groups) { + for (Action action : group.getActions()) { + + for (Group newGroup : newConfig.groups) { + for (Action newAction : newGroup.getActions()) { + + if (group.name.trim().equals(newGroup.name.trim()) && action.name.trim().equals(newAction.name.trim())) { + for (Variable variable : action.getVariables()) { + for (Variable newVariable : newAction.getVariables()) { + + if (variable.name.trim().equals(newVariable.name.trim())) { + variable.setValue(newVariable.rawValue()); + } + } + } + } + } + } + } + } + } } From 3cecfce1cf1d9c3f6bac2ff5a6f63f3354144ce3 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Mon, 11 Dec 2023 10:56:30 -0600 Subject: [PATCH 2/5] Stub autonomous for Red Crab minibot --- .../minibots/red_crab/RedCrabMinibot.java | 14 ++++ .../RedCrabAutonomousBlueAudienceEngine.java | 22 +++++++ .../RedCrabAutonomousBlueBackstageEngine.java | 22 +++++++ .../RedCrabAutonomousRedAudienceEngine.java | 22 +++++++ .../RedCrabAutonomousRedBackstageEngine.java | 22 +++++++ .../minibots/red_crab/states/Rotate.java | 65 +++++++++++++++++++ .../minibots/red_crab/states/StrafeMove.java | 19 ++++++ 7 files changed, 186 insertions(+) create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/StrafeMove.java diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index 9ba3884..0718295 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -2,6 +2,7 @@ package dev.cyberarm.minibots.red_crab; import com.arcrobotics.ftclib.hardware.motors.Motor; import com.arcrobotics.ftclib.hardware.motors.MotorEx; +import com.arcrobotics.ftclib.hardware.motors.MotorGroup; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.IMU; @@ -9,6 +10,9 @@ import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit; +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import java.sql.Time; import dev.cyberarm.engine.V2.CyberarmEngine; import dev.cyberarm.engine.V2.Utilities; @@ -58,11 +62,17 @@ public class RedCrabMinibot { public final MotorEx frontLeft, frontRight, backLeft, backRight, winch, clawArm; public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm; + public final MotorGroup left, right; + final CyberarmEngine engine; + public final TimeCraftersConfiguration config; + public RedCrabMinibot() { engine = CyberarmEngine.instance; + config = new TimeCraftersConfiguration("cyberarm_RedCrab"); + /// IMU /// /// ------------------------------------------------------------------------------------ /// imu = engine.hardwareMap.get(IMU.class, "imu"); // | Ctrl Hub, I2C Port: 0 @@ -98,6 +108,10 @@ public class RedCrabMinibot { backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE); backRight.setZeroPowerBehavior((Motor.ZeroPowerBehavior.BRAKE)); + /// --- MOTOR GROUPS + left = new MotorGroup(frontLeft, backLeft); + right = new MotorGroup(frontRight, backRight); + /// WINCH /// /// ------------------------------------------------------------------------------------ /// winch = new MotorEx(engine.hardwareMap, "winch"); // | Ctrl|Ex Hub, Port: ?? diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java new file mode 100644 index 0000000..78f857f --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueAudienceEngine.java @@ -0,0 +1,22 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +@Autonomous(name = "Cyberarm Red Crab BLUE AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") +public class RedCrabAutonomousBlueAudienceEngine extends CyberarmEngine { + @Override + public void setup() { + setupFromConfig( + new TimeCraftersConfiguration("cyberarm_RedCrab"), + "dev.cyberarm.minibots.red_crab.states", + new RedCrabMinibot(), + RedCrabMinibot.class, + "Autonomous_BLUE_Audience" + ); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java new file mode 100644 index 0000000..e4b080c --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousBlueBackstageEngine.java @@ -0,0 +1,22 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +@Autonomous(name = "Cyberarm Red Crab BLUE BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") +public class RedCrabAutonomousBlueBackstageEngine extends CyberarmEngine { + @Override + public void setup() { + setupFromConfig( + new TimeCraftersConfiguration("cyberarm_RedCrab"), + "dev.cyberarm.minibots.red_crab.states", + new RedCrabMinibot(), + RedCrabMinibot.class, + "Autonomous_BLUE_Backstage" + ); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java new file mode 100644 index 0000000..46271b6 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedAudienceEngine.java @@ -0,0 +1,22 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +@Autonomous(name = "Cyberarm Red Crab RED AUDIENCE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") +public class RedCrabAutonomousRedAudienceEngine extends CyberarmEngine { + @Override + public void setup() { + setupFromConfig( + new TimeCraftersConfiguration("cyberarm_RedCrab"), + "dev.cyberarm.minibots.red_crab.states", + new RedCrabMinibot(), + RedCrabMinibot.class, + "Autonomous_RED_Audience" + ); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java new file mode 100644 index 0000000..a285070 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousRedBackstageEngine.java @@ -0,0 +1,22 @@ +package dev.cyberarm.minibots.red_crab.engines; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration; + +import dev.cyberarm.engine.V2.CyberarmEngine; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +@Autonomous(name = "Cyberarm Red Crab RED BACKSTAGE", group = "MINIBOT", preselectTeleOp = "Cyberarm Red Crab TeleOp") +public class RedCrabAutonomousRedBackstageEngine extends CyberarmEngine { + @Override + public void setup() { + setupFromConfig( + new TimeCraftersConfiguration("cyberarm_RedCrab"), + "dev.cyberarm.minibots.red_crab.states", + new RedCrabMinibot(), + RedCrabMinibot.class, + "Autonomous_RED_Backstage" + ); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java new file mode 100644 index 0000000..e9d640f --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Rotate.java @@ -0,0 +1,65 @@ +package dev.cyberarm.minibots.red_crab.states; + +import com.qualcomm.robotcore.util.Range; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.engine.V2.Utilities; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; +import dev.cyberarm.minibots.yellow.YellowMinibot; + +public class Rotate extends CyberarmState { + final private RedCrabMinibot robot; + final private String groupName, actionName; + + final private double maxPower, minPower, lerpDegrees, headingDegrees, toleranceDegrees; + final private int timeoutMS; + + public Rotate(RedCrabMinibot robot, String groupName, String actionName) { + this.robot = robot; + + this.groupName = groupName; + this.actionName = actionName; + + this.minPower = robot.config.variable(groupName, actionName, "minPower").value(); + this.maxPower = robot.config.variable(groupName, actionName, "maxPower").value(); + + this.lerpDegrees = robot.config.variable(groupName, actionName, "lerpDEGREES").value(); + this.headingDegrees = robot.config.variable(groupName, actionName, "headingDEGREES").value(); + this.toleranceDegrees = robot.config.variable(groupName, actionName, "toleranceDEGREEES").value(); + + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + } + + @Override + public void exec() { + double angleDiff = Utilities.angleDiff(Utilities.facing(robot.imu) + Utilities.turnRate(robot.imu), headingDegrees); + + if (Math.abs(angleDiff) <= toleranceDegrees || runTime() >= timeoutMS) { + robot.left.set(0); + robot.right.set(0); + + this.finished(); + + return; + } + + double power = Utilities.lerp(minPower, maxPower, Range.clip(Math.abs(angleDiff) / lerpDegrees, 0.0, 1.0)); + + if (angleDiff > 0) { + robot.left.set(-power); + robot.right.set(power); + } else { + robot.left.set(power); + robot.right.set(-power); + } + } + + @Override + public void telemetry() { + engine.telemetry.addLine(); + engine.telemetry.addData("Robot Heading", Utilities.facing(robot.imu)); + engine.telemetry.addData("Robot Target Heading", headingDegrees); + engine.telemetry.addData("Robot Angle Diff", Utilities.angleDiff(Utilities.facing(robot.imu) + Utilities.turnRate(robot.imu), headingDegrees)); + engine.telemetry.addData("Robot Turn Rate", Utilities.turnRate(robot.imu)); + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/StrafeMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/StrafeMove.java new file mode 100644 index 0000000..7bfd28a --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/StrafeMove.java @@ -0,0 +1,19 @@ +package dev.cyberarm.minibots.red_crab.states; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class StrafeMove extends CyberarmState { + private final RedCrabMinibot robot; + private final String groupName, actionName; + public StrafeMove(RedCrabMinibot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + } + + @Override + public void exec() { + + } +} From 31c280e9c1aae2c06d68548ba086164ae740d474 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Mon, 11 Dec 2023 17:03:08 -0600 Subject: [PATCH 3/5] Implemented Move state for RedCrab --- .../minibots/red_crab/states/Move.java | 161 ++++++++++++++++++ .../minibots/red_crab/states/StrafeMove.java | 19 --- 2 files changed, 161 insertions(+), 19 deletions(-) create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java delete mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/StrafeMove.java diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java new file mode 100644 index 0000000..5593b61 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java @@ -0,0 +1,161 @@ +package dev.cyberarm.minibots.red_crab.states; + +import com.qualcomm.robotcore.util.Range; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.engine.V2.Utilities; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class Move extends CyberarmState { + private final RedCrabMinibot robot; + private final String groupName, actionName; + private final double distanceMM, lerpMM_UP, lerpMM_DOWN, maxPower, minPower, toleranceMM; + private boolean strafe = false; + private final int timeoutMS; + private double initialHeadingDegrees = 1024.0; + public Move(RedCrabMinibot robot, String groupName, String actionName) { + this.robot = robot; + this.groupName = groupName; + this.actionName = actionName; + + this.distanceMM = robot.config.variable(groupName, actionName, "distanceMM").value(); + this.lerpMM_UP = robot.config.variable(groupName, actionName, "lerpMM_UP").value(); + this.lerpMM_DOWN = robot.config.variable(groupName, actionName, "lerpMM_DOWN").value(); + this.toleranceMM = robot.config.variable(groupName, actionName, "toleranceMM").value(); + + this.maxPower = robot.config.variable(groupName, actionName, "maxPower").value(); + this.minPower = robot.config.variable(groupName, actionName, "minPower").value(); + + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + + // Validate distance and lerp distances + if (lerpMM_UP == 0 && lerpMM_DOWN == 0) { return; } + + // --- Good lerp UP distance? + if (Math.abs(distanceMM) - lerpMM_UP < 0) { + throw(new RuntimeException("Invalid lerp UP distance")); + // --- Good lerp DOWN distance? + } else if (Math.abs(distanceMM) - (lerpMM_UP + lerpMM_DOWN) < 0) { + throw(new RuntimeException("Invalid lerp distance(s)")); + } + } + + @Override + public void start() { + initialHeadingDegrees = Utilities.facing(robot.imu); + + if (initialHeadingDegrees > 365.0) { + throw(new RuntimeException("INVALID Initial IMU value!")); + } + + if (strafe) { + robot.frontLeft.resetEncoder(); + robot.frontRight.resetEncoder(); + robot.backLeft.resetEncoder(); + robot.backRight.resetEncoder(); + + robot.frontLeft.setPositionTolerance(toleranceMM); + robot.frontRight.setPositionTolerance(toleranceMM); + robot.backLeft.setPositionTolerance(toleranceMM); + robot.backRight.setPositionTolerance(toleranceMM); + + robot.frontLeft.setTargetDistance(distanceMM); + robot.frontRight.setTargetDistance(-distanceMM); + robot.backLeft.setTargetDistance(-distanceMM); + robot.backRight.setTargetDistance(distanceMM); + } else { + robot.left.resetEncoder(); + robot.right.resetEncoder(); + + robot.left.setPositionTolerance(toleranceMM); + robot.right.setPositionTolerance(toleranceMM); + + robot.left.setTargetDistance(distanceMM); + robot.right.setTargetDistance(distanceMM); + } + } + + @Override + public void exec() { + if (strafe) { + strafeMove(); + } else { + tankMove(); + } + } + + private void tankMove(){ + double travelledDistance = Math.abs(robot.left.getDistance()); + double power = lerpPower(travelledDistance); + + double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu)); + + double leftPower = power; + double rightPower = power; + // use +10% of power at 7 degrees of error to correct angle + double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power + power * 0.1); + if (angleDiff < -0.5) { + leftPower += correctivePower; + } else if (angleDiff > 0.5) { + rightPower += correctivePower; + } + + robot.left.set(leftPower); + robot.right.set(rightPower); + + if (robot.left.atTargetPosition() && robot.right.atTargetPosition()) { + robot.left.set(0); + robot.right.set(0); + + this.finished(); + } + } + + private void strafeMove() { + double travelledDistance = Math.abs(robot.frontLeft.getDistance()); + double power = lerpPower(travelledDistance); + + double angleDiff = Utilities.angleDiff(initialHeadingDegrees, Utilities.facing(robot.imu)); + + double frontPower = power; + double backPower = power; + // use +10% of power at 7 degrees of error to correct angle + double correctivePower = Utilities.lerp(0.0, 1.0, angleDiff / 7.0) * (power + power * 0.1); + if (angleDiff < -0.5) { + frontPower += correctivePower; + } else if (angleDiff > 0.5) { + backPower += correctivePower; + } + + robot.frontLeft.set(frontPower); + robot.frontRight.set(-frontPower); + robot.backLeft.set(-backPower); + robot.backRight.set(backPower); + + if (robot.frontLeft.atTargetPosition() && robot.backRight.atTargetPosition()) { + robot.frontLeft.set(0); + robot.frontRight.set(0); + robot.backLeft.set(0); + robot.backRight.set(0); + + this.finished(); + } + } + + private double lerpPower(double travelledDistance) { + double lerpPower = maxPower; + + // Ease power up + if (travelledDistance < lerpMM_UP) { // Not using <= to prevent divide by zero + lerpPower = Utilities.lerp(minPower, maxPower, Range.clip(travelledDistance / lerpMM_UP, 0.0, 1.0)); + // Cruising power + } else if (travelledDistance < Math.abs(distanceMM) - lerpMM_DOWN) { + lerpPower = maxPower; + // Ease power down + } else { + lerpPower = Utilities.lerp(minPower, maxPower, Range.clip( (Math.abs(distanceMM) - travelledDistance) / lerpMM_DOWN, 0.0, 1.0)); + } + + return lerpPower; + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/StrafeMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/StrafeMove.java deleted file mode 100644 index 7bfd28a..0000000 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/StrafeMove.java +++ /dev/null @@ -1,19 +0,0 @@ -package dev.cyberarm.minibots.red_crab.states; - -import dev.cyberarm.engine.V2.CyberarmState; -import dev.cyberarm.minibots.red_crab.RedCrabMinibot; - -public class StrafeMove extends CyberarmState { - private final RedCrabMinibot robot; - private final String groupName, actionName; - public StrafeMove(RedCrabMinibot robot, String groupName, String actionName) { - this.robot = robot; - this.groupName = groupName; - this.actionName = actionName; - } - - @Override - public void exec() { - - } -} From 451a28994d0a4664ade75fbbbfe09502f10bb566 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Mon, 11 Dec 2023 17:45:43 -0600 Subject: [PATCH 4/5] Implemented ServoMove state for RedCrab with lerp-ing support --- .../minibots/red_crab/states/Move.java | 14 +++++ .../minibots/red_crab/states/ServoMove.java | 63 +++++++++++++++++++ 2 files changed, 77 insertions(+) create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ServoMove.java diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java index 5593b61..d363b6b 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java @@ -26,6 +26,8 @@ public class Move extends CyberarmState { this.maxPower = robot.config.variable(groupName, actionName, "maxPower").value(); this.minPower = robot.config.variable(groupName, actionName, "minPower").value(); + this.strafe = robot.config.variable(groupName, actionName, "strafe").value(); + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); // Validate distance and lerp distances @@ -84,6 +86,18 @@ public class Move extends CyberarmState { } } + @Override + public void telemetry() { + engine.telemetry.addLine(); + engine.telemetry.addData("Strafing", strafe); + engine.telemetry.addData("lerp MM UP", lerpMM_UP); + engine.telemetry.addData("lerp MM DOWN", lerpMM_DOWN); + engine.telemetry.addData("Distance MM", distanceMM); + engine.telemetry.addData("Distance Travelled MM", (strafe ? robot.left.getDistance() : robot.frontLeft.getDistance())); + engine.telemetry.addData("Timeout MS", timeoutMS); + progressBar(20, runTime() / timeoutMS); + } + private void tankMove(){ double travelledDistance = Math.abs(robot.left.getDistance()); double power = lerpPower(travelledDistance); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ServoMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ServoMove.java new file mode 100644 index 0000000..14f0847 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ServoMove.java @@ -0,0 +1,63 @@ +package dev.cyberarm.minibots.red_crab.states; + +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.engine.V2.Utilities; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class ServoMove extends CyberarmState { + private final RedCrabMinibot robot; + private final Servo servo; + private final double startingPosition, targetPosition, lerpMS; + private final int timeoutMS; + private final boolean lerp; + private final String servoName; + public ServoMove(RedCrabMinibot robot, String groupName, String actionName) { + this.robot = robot; + + this.servoName = robot.config.variable(groupName, actionName, "servoName").value(); + this.targetPosition = robot.config.variable(groupName, actionName, "position").value(); + this.lerp = robot.config.variable(groupName, actionName, "lerp").value(); + this.lerpMS = robot.config.variable(groupName, actionName, "lerpMS").value(); + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + + this.servo = engine.hardwareMap.servo.get(servoName); + this.startingPosition = this.servo.getPosition(); + } + + @Override + public void exec() { + if (lerp) { + servo.setPosition( + Utilities.lerp( + startingPosition, + targetPosition, + Range.clip(runTime() / lerpMS, 0.0, 1.0)) + ); + } else { + servo.setPosition(targetPosition); + } + + if (runTime() >= timeoutMS) { + servo.setPosition(targetPosition); + this.finished(); + } + } + + @Override + public void telemetry() { + engine.telemetry.addLine(); + engine.telemetry.addData("Servo", servoName); + engine.telemetry.addData("Servo Position", servo.getPosition()); + engine.telemetry.addData("Servo Starting Position", startingPosition); + engine.telemetry.addData("Servo Target Position", targetPosition); + engine.telemetry.addData("lerp", lerp); + engine.telemetry.addData("lerp MS", lerpMS); + progressBar(20, runTime() / lerpMS); + engine.telemetry.addLine(); + engine.telemetry.addData("Timeout MS", timeoutMS); + progressBar(20, runTime() / timeoutMS); + } +} From d792a00c4e5cc0fcf12ff0d845d5459979b39d69 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Mon, 11 Dec 2023 18:32:54 -0600 Subject: [PATCH 5/5] Implemented ClawArmMove state for RedCrab --- .../minibots/red_crab/states/ClawArmMove.java | 48 +++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java new file mode 100644 index 0000000..66b1f37 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java @@ -0,0 +1,48 @@ +package dev.cyberarm.minibots.red_crab.states; + +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +import dev.cyberarm.engine.V2.CyberarmState; +import dev.cyberarm.engine.V2.Utilities; +import dev.cyberarm.minibots.red_crab.RedCrabMinibot; + +public class ClawArmMove extends CyberarmState { + private final RedCrabMinibot robot; + private final double power, targetAngle, toleranceAngle, gearRatio; + private final int timeoutMS, motorTicks; + public ClawArmMove(RedCrabMinibot robot, String groupName, String actionName) { + this.robot = robot; + + this.targetAngle = robot.config.variable(groupName, actionName, "angle").value(); + this.power = robot.config.variable(groupName, actionName, "power").value(); + this.toleranceAngle = robot.config.variable(groupName, actionName, "toleranceAngle").value(); + this.timeoutMS = robot.config.variable(groupName, actionName, "timeoutMS").value(); + + this.motorTicks = RedCrabMinibot.CLAW_ARM_MOTOR_TICKS_PER_REVOLUTION; + this.gearRatio = RedCrabMinibot.CLAW_ARM_MOTOR_GEAR_RATIO; + } + + @Override + public void start() { + robot.clawArm.setPositionTolerance(Utilities.motorAngle(motorTicks, gearRatio, toleranceAngle)); + robot.clawArm.setTargetPosition(Utilities.motorAngle(motorTicks, gearRatio, targetAngle)); + robot.clawArm.set(power); + } + + @Override + public void exec() { + if (robot.clawArm.atTargetPosition() || runTime() >= timeoutMS) { + this.finished(); + } + } + + @Override + public void telemetry() { + engine.telemetry.addLine(); + engine.telemetry.addData("Motor Position", robot.clawArm.getCurrentPosition()); + engine.telemetry.addData("Motor Angle", Utilities.motorAngle(motorTicks, gearRatio, robot.clawArm.getCurrentPosition())); + engine.telemetry.addData("Timeout MS", timeoutMS); + progressBar(20, runTime() / timeoutMS); + } +}