dampening to prevent occilations
This commit is contained in:
parent
73c2cfb056
commit
6bb0d1796e
|
@ -1,48 +0,0 @@
|
||||||
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();
|
|
||||||
}
|
|
||||||
}
|
|
|
@ -0,0 +1,85 @@
|
||||||
|
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");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
|
@ -68,20 +68,15 @@ public class EncoderIntegrator {
|
||||||
double brDiff = newBr - this.br;
|
double brDiff = newBr - this.br;
|
||||||
double imuYawDiff = newImuYaw - this.imuYaw;
|
double imuYawDiff = newImuYaw - this.imuYaw;
|
||||||
|
|
||||||
// telemetry.addData("flDiff", flDiff);
|
// fl = powerY + powerX + turn + noop
|
||||||
// telemetry.addData("frDiff", frDiff);
|
// fr = powerY - powerX - turn + noop
|
||||||
// telemetry.addData("blDiff", blDiff);
|
// bl = powerY - powerX + turn - noop
|
||||||
// telemetry.addData("brDiff", brDiff);
|
// br = powerY + powerX - turn - noop
|
||||||
// telemetry.addData("imuYawDiff", imuYawDiff);
|
|
||||||
|
|
||||||
// fl = powerY + powerX + turn + noop
|
|
||||||
// fr = powerY - powerX - turn + noop
|
|
||||||
// bl = powerY - powerX + turn - noop
|
|
||||||
// br = powerY + powerX - turn - noop
|
|
||||||
|
|
||||||
double relativeYDiff = ((flDiff + frDiff + blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
|
double relativeYDiff = ((flDiff + frDiff + blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
|
||||||
double relativeXDiff = ((flDiff - frDiff - blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
|
double relativeXDiff = ((flDiff - frDiff - blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
|
||||||
double turnDiff = ((flDiff - frDiff + blDiff - brDiff) / 4.) * MM_PER_ENCODER_TICK * DEGREES_PER_MM;
|
double turnDiff = ((flDiff - frDiff + blDiff - brDiff) / 4.) * MM_PER_ENCODER_TICK * DEGREES_PER_MM;
|
||||||
|
turnDiff *= WHEELBASE_DIAGONAL / WHEELBASE_WIDTH;
|
||||||
double noopDiff = (flDiff + frDiff - blDiff - brDiff) / 4.;
|
double noopDiff = (flDiff + frDiff - blDiff - brDiff) / 4.;
|
||||||
|
|
||||||
// aaaaand then we are just going to ignore `turnDiff` and `noopDiff`. there is nothing
|
// aaaaand then we are just going to ignore `turnDiff` and `noopDiff`. there is nothing
|
||||||
|
@ -89,9 +84,6 @@ public class EncoderIntegrator {
|
||||||
// is ignored for obvious reasons. the turn value is ignored since we get the turn from the
|
// is ignored for obvious reasons. the turn value is ignored since we get the turn from the
|
||||||
// imu and using encoders for turn has proven to work poorly.
|
// imu and using encoders for turn has proven to work poorly.
|
||||||
|
|
||||||
// telemetry.addData("relativeYDiff", relativeYDiff);
|
|
||||||
// telemetry.addData("relativeXDiff", relativeXDiff);
|
|
||||||
|
|
||||||
double imuYawDiffRadians = Math.toRadians(imuYawDiff);
|
double imuYawDiffRadians = Math.toRadians(imuYawDiff);
|
||||||
double poseExponentiationX = cosc(imuYawDiffRadians);
|
double poseExponentiationX = cosc(imuYawDiffRadians);
|
||||||
double poseExponentiationY = sinc(imuYawDiffRadians);
|
double poseExponentiationY = sinc(imuYawDiffRadians);
|
||||||
|
@ -112,6 +104,5 @@ public class EncoderIntegrator {
|
||||||
this.bl = newBl;
|
this.bl = newBl;
|
||||||
this.br = newBr;
|
this.br = newBr;
|
||||||
this.imuYaw = newImuYaw;
|
this.imuYaw = newImuYaw;
|
||||||
this.imuYaw %= 360;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -13,10 +13,6 @@ public enum LiftPosition {
|
||||||
public static int MEDIUM_ENCODER_VAL = 2000;
|
public static int MEDIUM_ENCODER_VAL = 2000;
|
||||||
public static int HIGH_ENCODER_VAL = 2900;
|
public static int HIGH_ENCODER_VAL = 2900;
|
||||||
|
|
||||||
LiftPosition() {
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
public int getEncoderVal() {
|
public int getEncoderVal() {
|
||||||
switch (this) {
|
switch (this) {
|
||||||
case FLOOR:
|
case FLOOR:
|
||||||
|
|
|
@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.Hardware;
|
||||||
@Config
|
@Config
|
||||||
public class Poser {
|
public class Poser {
|
||||||
Hardware hardware;
|
Hardware hardware;
|
||||||
AccelLimiter movement;
|
Dampener movement;
|
||||||
Telemetry telemetry;
|
Telemetry telemetry;
|
||||||
EncoderIntegrator integrator;
|
EncoderIntegrator integrator;
|
||||||
|
|
||||||
|
@ -31,7 +31,7 @@ public class Poser {
|
||||||
public Poser(Hardware hardware, double speed, FieldVector initalPos, double initialYaw) {
|
public Poser(Hardware hardware, double speed, FieldVector initalPos, double initialYaw) {
|
||||||
this.hardware = hardware;
|
this.hardware = hardware;
|
||||||
hardware.initIMU();
|
hardware.initIMU();
|
||||||
this.movement = new AccelLimiter(hardware);
|
this.movement = new Dampener(hardware);
|
||||||
this.speed = speed;
|
this.speed = speed;
|
||||||
this.telemetry = hardware.opMode.telemetry;
|
this.telemetry = hardware.opMode.telemetry;
|
||||||
this.integrator = new EncoderIntegrator(hardware, initalPos, initialYaw, telemetry);
|
this.integrator = new EncoderIntegrator(hardware, initalPos, initialYaw, telemetry);
|
||||||
|
|
Loading…
Reference in a new issue