TimeCrafters/FTC_2022
1
0
Fork 0
mirror of https://github.com/TimeCrafters/FTC_2022 synced 2025-12-13 06:22:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Rework Arm state to be able to use 'tuning' variables for arm angle

Browse Source
This commit is contained in:
Cyberarm
2023-01-29 18:25:58 -06:00
parent 105b31a31b
commit 513733965a
1 changed files with 17 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Arm.java
View File

@@ -8,8 +8,9 @@ public class Arm extends CyberarmState {
private final String groupName, actionName;
private final double targetVelocity, timeInMS;
private final int tolerance, halfTolerance, targetPosition;
private final boolean stateDisabled;
private final int tolerance, halfTolerance, position, manualPosition;
private final boolean positionManually, stateDisabled;
private final String positionLookupLabel;
public Arm(Robot robot, String groupName, String actionName) {
this.robot = robot;
@@ -20,18 +21,27 @@ public class Arm extends CyberarmState {
robot.getConfiguration().variable(groupName, actionName, "targetVelocityInDegreesPerSecond").value());
tolerance = robot.angleToTicks(
robot.getConfiguration().variable(groupName, actionName, "toleranceInDegrees").value());
targetPosition = robot.angleToTicks(
robot.getConfiguration().variable(groupName, actionName, "targetAngle").value());
timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
positionLookupLabel = robot.getConfiguration().variable(groupName, actionName, "positionLookupLabel").value();
positionManually = robot.getConfiguration().variable(groupName, actionName, "positionManually").value();
manualPosition = robot.angleToTicks(
robot.getConfiguration().variable(groupName, actionName, "manualTargetAngle").value());
stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
halfTolerance = Math.round(tolerance / 2.0f);
if (positionManually) {
position = manualPosition;
} else {
position = robot.angleToTicks(robot.tuningConfig(positionLookupLabel).value());
}
}
@Override
public void start() {
robot.arm.setTargetPosition(targetPosition);
robot.arm.setTargetPosition(position);
robot.arm.setTargetPositionTolerance(tolerance);
robot.arm.setVelocity(targetVelocity);
}
@@ -52,8 +62,8 @@ public class Arm extends CyberarmState {
return;
}
int position = robot.arm.getCurrentPosition();
if (robot.isBetween(position, position - halfTolerance, position + halfTolerance)) {
int currentPosition = robot.arm.getCurrentPosition();
if (robot.isBetween(currentPosition, currentPosition - halfTolerance, currentPosition + halfTolerance)) {
setHasFinished(true);
}
}