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); + } +}