missing/power-play
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

dampening to prevent occilations

Browse source
This commit is contained in:
missing 2023-01-26 13:05:16 -06:00
parent 73c2cfb056
commit 6bb0d1796e
5 changed files with 92 additions and 68 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

48
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/AccelLimiter.java
View file

@ -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();
}
}

85
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Dampener.java Normal file
View file

@ -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");
}
}
}

19
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java
View file

@ -68,20 +68,15 @@ public class EncoderIntegrator {
double brDiff = newBr - this.br;
double imuYawDiff = newImuYaw - this.imuYaw;
// telemetry.addData("flDiff", flDiff);
// telemetry.addData("frDiff", frDiff);
// telemetry.addData("blDiff", blDiff);
// telemetry.addData("brDiff", brDiff);
// telemetry.addData("imuYawDiff", imuYawDiff);
// fl = powerY + powerX + turn + noop
// fr = powerY - powerX - turn + noop
// bl = powerY - powerX + turn - noop
// br = powerY + powerX - turn - noop
// 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 relativeXDiff = ((flDiff - frDiff - blDiff + brDiff) / 4.) * MM_PER_ENCODER_TICK;
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.;
// 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
// 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 poseExponentiationX = cosc(imuYawDiffRadians);
double poseExponentiationY = sinc(imuYawDiffRadians);
@ -112,6 +104,5 @@ public class EncoderIntegrator {
this.bl = newBl;
this.br = newBr;
this.imuYaw = newImuYaw;
this.imuYaw %= 360;
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java
View file

@ -13,10 +13,6 @@ public enum LiftPosition {
public static int MEDIUM_ENCODER_VAL = 2000;
public static int HIGH_ENCODER_VAL = 2900;
LiftPosition() {
}
public int getEncoderVal() {
switch (this) {
case FLOOR:

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java
View file

@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.Hardware;
@Config
public class Poser {
Hardware hardware;
AccelLimiter movement;
Dampener movement;
Telemetry telemetry;
EncoderIntegrator integrator;
@ -31,7 +31,7 @@ public class Poser {
public Poser(Hardware hardware, double speed, FieldVector initalPos, double initialYaw) {
this.hardware = hardware;
hardware.initIMU();
this.movement = new AccelLimiter(hardware);
this.movement = new Dampener(hardware);
this.speed = speed;
this.telemetry = hardware.opMode.telemetry;
this.integrator = new EncoderIntegrator(hardware, initalPos, initialYaw, telemetry);