TimeCrafters/CenterStage
1
0
Fork 0
mirror of https://github.com/TimeCrafters/CenterStage synced 2025-12-15 08:42:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Implemented ServoMove state for RedCrab with lerp-ing support

Browse Source
This commit is contained in:
Cyberarm
2023-12-11 17:45:43 -06:00
parent 31c280e9c1
commit 451a28994d
2 changed files with 77 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/Move.java
View File

@@ -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);

63
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ServoMove.java Normal file
View File

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