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

Compare commits

base: TimeCrafters:9855c1e0ca5137986e9e7230601a8dcae4376798
Branches Tags
TimeCrafters:master
..
compare: TimeCrafters:513733965a50cb44684c5134e2dcb5156d404586
Branches Tags
TimeCrafters:master

4 Commits
9855c1e0ca ... 513733965a

Author SHA1 Message Date
Cyberarm
513733965a Rework Arm state to be able to use 'tuning' variables for arm angle 2023-01-29 18:25:58 -06:00
Cyberarm
105b31a31b Add Wait state 2023-01-29 17:50:44 -06:00
Cyberarm
4b04dc2799 Finish Arm state when Arm arrives 2023-01-29 17:47:57 -06:00
cyberarm
2a9ac57313 Lerp ease in/out velocity for Move 2023-01-29 17:38:30 -06:00
4 changed files with 71 additions and 10 deletions
Expand all files Collapse all files

5
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/Robot.java
View File

@@ -563,6 +563,11 @@ public class Robot {
return (fmod < 0 ? fmod + 360.0 : fmod + 0.0) - 180;
}
public double lerp(double min, double max, double t)
{
return min + (max - min) * t;
}
public Status getStatus() { return status; }
public double getRadius() { return radius; }

27
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, 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,16 +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);
}
@@ -49,5 +61,10 @@ public class Arm extends CyberarmState {
return;
}
int currentPosition = robot.arm.getCurrentPosition();
if (robot.isBetween(currentPosition, currentPosition - halfTolerance, currentPosition + halfTolerance)) {
setHasFinished(true);
}
}
}

10
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Move.java
View File

@@ -1,5 +1,7 @@
package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
import com.qualcomm.robotcore.util.Range;
import org.cyberarm.engine.V2.CyberarmState;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.timecrafters.minibots.cyberarm.chiron.Robot;
@@ -72,8 +74,7 @@ public class Move extends CyberarmState {
}
private void moveDirectional(double travelledDistance) {
double velocity = targetVelocity;
double easeVelocity;
double velocity;
double ratio = 1.0;
if (Math.abs(travelledDistance) < easeInDistance) {
@@ -82,10 +83,9 @@ public class Move extends CyberarmState {
ratio = (targetDistance - Math.abs(travelledDistance)) / easeOutDistance;
}
easeVelocity = targetVelocity * ratio;
ratio = Range.clip(ratio, 0.0, 1.0);
if (easeVelocity < minimumVelocity) { easeVelocity = minimumVelocity; }
if (easeVelocity < targetVelocity) { velocity = easeVelocity; }
velocity = robot.lerp(minimumVelocity, targetVelocity, ratio);
if (targetDistance > travelledDistance) {
robot.frontRightDrive.setVelocity(-velocity);

39
TeamCode/src/main/java/org/timecrafters/minibots/cyberarm/chiron/states/autonomous/Wait.java Normal file
View File

@@ -0,0 +1,39 @@
package org.timecrafters.minibots.cyberarm.chiron.states.autonomous;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.minibots.cyberarm.chiron.Robot;
public class Wait extends CyberarmState {
private final Robot robot;
private final String groupName, actionName;
private final double timeInMS;
private final boolean stateDisabled;
public Wait(Robot robot, String groupName, String actionName) {
this.robot = robot;
this.groupName = groupName;
this.actionName = actionName;
timeInMS = robot.getConfiguration().variable(groupName, actionName, "timeInMS").value();
stateDisabled = !robot.getConfiguration().action(groupName, actionName).enabled;
}
@Override
public void exec() {
if (stateDisabled) {
setHasFinished(true);
return;
}
if (runTime() >= timeInMS) {
stop();
setHasFinished(true);
return;
}
}
}