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

Implemented ClawArmMove state for RedCrab

Browse Source
This commit is contained in:
Cyberarm
2023-12-11 18:32:54 -06:00
parent 451a28994d
commit d792a00c4e
1 changed files with 48 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

48
TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/states/ClawArmMove.java Normal file
View File

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