86 lines
2.9 KiB
Java
86 lines
2.9 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 Dampener {
|
|
private final Hardware hardware;
|
|
|
|
// dampening
|
|
private double[] posPowerHist;
|
|
private double[] yawPowerHist;
|
|
|
|
// accel limiting
|
|
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 Dampener(Hardware hardware) {
|
|
this.hardware = hardware;
|
|
|
|
this.posPowerHist = new double[4];
|
|
this.yawPowerHist = new double[4];
|
|
|
|
this.posPower = 0;
|
|
this.yawPower = 0;
|
|
this.lastUpdateTime = System.nanoTime();
|
|
}
|
|
|
|
private static double shiftBackAverage(double[] arr, double x) {
|
|
double total = 0;
|
|
for (int i = 0; i < arr.length - 1; i++) {
|
|
total += arr[i] = arr[i + 1];
|
|
}
|
|
total += arr[arr.length - 1] = x;
|
|
return total / arr.length;
|
|
}
|
|
|
|
public void move(double powerX, double powerY, double turn, double speed) {
|
|
// we like to work with the magnitude of the position velocity vector instead of the individual components
|
|
double hypot = Math.hypot(powerX, powerY);
|
|
double powerXNormalized = powerX / hypot;
|
|
double powerYNormalized = powerY / hypot;
|
|
|
|
// delta time
|
|
double time = System.nanoTime() / NANOS_PER_SEC;
|
|
double dt = time - this.lastUpdateTime;
|
|
this.lastUpdateTime = time;
|
|
|
|
// dampen
|
|
double posPower = shiftBackAverage(this.posPowerHist, hypot);
|
|
double yawPower = shiftBackAverage(this.yawPowerHist, turn);
|
|
|
|
// limit accel
|
|
double posAccelLimit = MAX_POS_ACCELERATION * dt;
|
|
double yawAccelLimit = MAX_YAW_ACCELERATION * dt;
|
|
this.posPower += Range.clip(posPower - this.posPower, -posAccelLimit, posAccelLimit);
|
|
this.yawPower += Range.clip(yawPower - this.yawPower, -yawAccelLimit, yawAccelLimit);
|
|
|
|
// then we finally move
|
|
this.hardware.move(powerXNormalized * this.posPower, powerYNormalized * this.posPower, this.yawPower, speed);
|
|
}
|
|
|
|
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();
|
|
|
|
double time = System.nanoTime() / NANOS_PER_SEC;
|
|
double dt = time - this.lastUpdateTime;
|
|
|
|
double posLimit = MAX_POS_ACCELERATION * dt;
|
|
double yawLimit = MAX_YAW_ACCELERATION * dt;
|
|
|
|
if (Math.abs(this.posPower) > posLimit || Math.abs(this.yawPower) > yawLimit) {
|
|
hardware.opMode.telemetry.log().add("warning: robot is likely experiencing deceleration slippage");
|
|
}
|
|
}
|
|
}
|