49 lines
1.5 KiB
Java
49 lines
1.5 KiB
Java
package org.firstinspires.ftc.teamcode.kinematics;
|
|
|
|
import com.acmerobotics.dashboard.config.Config;
|
|
import com.qualcomm.robotcore.util.Range;
|
|
|
|
import org.firstinspires.ftc.teamcode.Hardware;
|
|
|
|
@Config
|
|
public class AccelLimiter {
|
|
private final Hardware hardware;
|
|
|
|
private double posPower;
|
|
private double yawPower;
|
|
private double lastUpdateTime;
|
|
|
|
public static double MAX_POS_ACCELERATION = 13;
|
|
public static double MAX_YAW_ACCELERATION = 1;
|
|
|
|
private static final double NANOS_PER_SEC = 1000 * 1000 * 1000;
|
|
|
|
public AccelLimiter(Hardware hardware) {
|
|
this.hardware = hardware;
|
|
this.posPower = 0;
|
|
this.yawPower = 0;
|
|
}
|
|
|
|
public void move(double powerX, double powerY, double turn, double speed) {
|
|
double hypot = Math.hypot(powerX, powerY);
|
|
|
|
double time = System.nanoTime() / NANOS_PER_SEC;
|
|
double dt = time - this.lastUpdateTime;
|
|
|
|
double posLimit = MAX_POS_ACCELERATION * dt;
|
|
double yawLimit = MAX_YAW_ACCELERATION * dt;
|
|
|
|
this.posPower += Range.clip(hypot - this.posPower, -posLimit, posLimit);
|
|
this.yawPower += Range.clip(turn - this.yawPower, -yawLimit, yawLimit);
|
|
|
|
this.hardware.move(powerX / hypot * this.posPower, powerY / hypot * this.posPower, this.yawPower, speed);
|
|
|
|
this.lastUpdateTime = time;
|
|
}
|
|
|
|
public void stop() {
|
|
// yes, yes, i know this isnt good, but for now its only called when power values are low anyway
|
|
this.hardware.stop();
|
|
}
|
|
}
|