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

Lerp ease in/out velocity for Move

Browse Source
This commit is contained in:
cyberarm
2023-01-29 17:38:30 -06:00
parent 9855c1e0ca
commit 2a9ac57313
2 changed files with 10 additions and 5 deletions
Download Patch File Download Diff File 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; 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 Status getStatus() { return status; }
public double getRadius() { return radius; } public double getRadius() { return radius; }

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