From 6bb0d1796e6e267c6898851b655f090cdfce164f Mon Sep 17 00:00:00 2001 From: missing Date: Thu, 26 Jan 2023 13:05:16 -0600 Subject: [PATCH] dampening to prevent occilations --- .../ftc/teamcode/kinematics/AccelLimiter.java | 48 ----------- .../ftc/teamcode/kinematics/Dampener.java | 85 +++++++++++++++++++ .../kinematics/EncoderIntegrator.java | 19 ++--- .../ftc/teamcode/kinematics/LiftPosition.java | 4 - .../ftc/teamcode/kinematics/Poser.java | 4 +- 5 files changed, 92 insertions(+), 68 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/AccelLimiter.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Dampener.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/AccelLimiter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/AccelLimiter.java deleted file mode 100644 index 8b6f9fd..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/AccelLimiter.java +++ /dev/null @@ -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(); - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Dampener.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Dampener.java new file mode 100644 index 0000000..023d666 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Dampener.java @@ -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"); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java index 4e167d0..58d6a75 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/EncoderIntegrator.java @@ -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; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java index ab0ecaf..28414e1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/LiftPosition.java @@ -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: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java index 0135193..e48cc2d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.java @@ -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);