From 5501c4bdc60b811ea0c35f04756d212f55b88424 Mon Sep 17 00:00:00 2001 From: missing Date: Wed, 25 Jan 2023 23:27:08 -0600 Subject: [PATCH] pid stuff --- .../teamcode/DashboardTelemetryWrapper.java | 13 +++-- .../firstinspires/ftc/teamcode/auto/Test.java | 20 +++++--- .../ftc/teamcode/kinematics/AccelLimiter.java | 48 ++++++++++++++++++ .../ftc/teamcode/kinematics/PID.java | 41 +++++++++++++++ .../ftc/teamcode/kinematics/Poser.java | 50 +++++++++++++------ 5 files changed, 143 insertions(+), 29 deletions(-) create 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/PID.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java index 481940a..8cf3619 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java @@ -22,6 +22,8 @@ public class DashboardTelemetryWrapper implements Telemetry { } private void rawDrawRobot(FieldVector pos, double yaw) { + pos = pos.mul(24 / Distance.IN_PER_TILE); + // 2--3 6--7 // | | | | // | 4----5 | @@ -32,8 +34,8 @@ public class DashboardTelemetryWrapper implements Telemetry { new FieldVector( Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH.neg() ).div(2), new FieldVector( Distance.ROBOT_WIDTH.neg(), Distance.ROBOT_LENGTH ).div(2), new FieldVector( Distance.ROBOT_WIDTH.neg().div(2), Distance.ROBOT_LENGTH ).div(2), - new FieldVector( Distance.ROBOT_WIDTH.neg().div(2), Distance.ROBOT_LENGTH.div(3) ).div(2), - new FieldVector( Distance.ROBOT_WIDTH.div(2), Distance.ROBOT_LENGTH.div(3) ).div(2), + new FieldVector( Distance.ROBOT_WIDTH.neg().div(2), Distance.ROBOT_LENGTH.div(5) ).div(2), + new FieldVector( Distance.ROBOT_WIDTH.div(2), Distance.ROBOT_LENGTH.div(5) ).div(2), new FieldVector( Distance.ROBOT_WIDTH.div(2), Distance.ROBOT_LENGTH ).div(2), new FieldVector( Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH ).div(2), new FieldVector( Distance.ROBOT_WIDTH, Distance.ROBOT_LENGTH.neg() ).div(2) @@ -49,6 +51,7 @@ public class DashboardTelemetryWrapper implements Telemetry { Canvas canvas = this.packet.fieldOverlay(); canvas.fillPolygon(xs, ys); + canvas.strokePolygon(xs, ys); } public void drawRobot(FieldVector pos, double yaw) { @@ -63,10 +66,10 @@ public class DashboardTelemetryWrapper implements Telemetry { public void drawTarget(Pose target, Pose current) { Canvas canvas = this.packet.fieldOverlay(); canvas.setStrokeWidth(1); - canvas.setFill("green"); - canvas.setStroke("green"); + canvas.setFill("lightgreen"); + canvas.setStroke("lightgreen"); - canvas.fillCircle(target.pos.x.valInInches(), target.pos.y.valInInches(), 2); + canvas.fillCircle(target.pos.x.valInInches(), target.pos.y.valInInches(), 1); canvas.strokeLine( current.pos.x.valInInches(), current.pos.y.valInInches(), diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java index 2f85d6d..a975741 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java @@ -4,7 +4,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.Hardware; -import org.firstinspires.ftc.teamcode.kinematics.LiftPosition; import org.firstinspires.ftc.teamcode.kinematics.NullablePose; import org.firstinspires.ftc.teamcode.kinematics.FieldVector; import org.firstinspires.ftc.teamcode.kinematics.Poser; @@ -18,19 +17,24 @@ public class Test extends LinearOpMode { public void runOpMode() { hardware = new Hardware(this); hardware.setWristPosition(-0.6); - poser = new Poser(hardware, 0.3, FieldVector.ZERO, 0); + poser = new Poser(hardware, 0.15, FieldVector.ZERO, 0); poser.goTo(new NullablePose()); waitForStart(); +// while (!isStopRequested()) { +// poser.goTo(new NullablePose(FieldVector.inTiles(2, 0), 90., LiftPosition.FLOOR)); +// sleep(1000); +// poser.goTo(new NullablePose(FieldVector.inTiles(0, 1), -135., LiftPosition.HIGH)); +// sleep(1000); +// poser.goTo(new NullablePose(FieldVector.inTiles(0, 0), 0., LiftPosition.MEDIUM)); +// sleep(1000); +// } + while (!isStopRequested()) { - poser.goTo(new NullablePose(FieldVector.inTiles(2, 0), 90., LiftPosition.FLOOR)); - sleep(1000); - poser.goTo(new NullablePose(FieldVector.inTiles(0, 1), -135., LiftPosition.HIGH)); - sleep(1000); - poser.goTo(new NullablePose(FieldVector.inTiles(0, 0), 0., LiftPosition.MEDIUM)); - sleep(1000); + poser.goTo(new NullablePose(FieldVector.inTiles(1, 0))); + poser.goTo(new NullablePose(FieldVector.inTiles(0, 0))); } } } 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 new file mode 100644 index 0000000..c11e175 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/AccelLimiter.java @@ -0,0 +1,48 @@ +package org.firstinspires.ftc.teamcode.kinematics; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.util.Range; + +import org.firstinspires.ftc.teamcode.MovementAPI; + +@Config +public class AccelLimiter { + private final MovementAPI movement; + + 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(MovementAPI inner) { + this.movement = inner; + 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.movement.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.movement.stop(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/PID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/PID.java new file mode 100644 index 0000000..3473799 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/PID.java @@ -0,0 +1,41 @@ +package org.firstinspires.ftc.teamcode.kinematics; + +import com.qualcomm.robotcore.hardware.PIDCoefficients; + +public class PID { + public final PIDCoefficients coefficients; + private double lastX; + private double lastUpdateTime; + private boolean firstIteration; + private double i; + + private static final double NANOS_PER_SEC = 1000 * 1000 * 1000; + + public PID(PIDCoefficients coefficients) { + this.coefficients = coefficients; + this.firstIteration = true; + this.i = 0; + } + + public double update(double x) { + double time = System.nanoTime() / NANOS_PER_SEC; + + double res = this.coefficients.p * x; + if (!this.firstIteration) { + double dt = time - this.lastUpdateTime; + + this.i += x * dt; + double d = (x - lastX) / dt; + + res += this.coefficients.i * this.i; + res += this.coefficients.d * d; + } else { + this.firstIteration = false; + } + + this.lastX = x; + this.lastUpdateTime = time; + + return res; + } +} 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 3ec67e1..258f066 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 @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.kinematics; import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.PIDCoefficients; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.teamcode.Hardware; @@ -9,23 +10,32 @@ import org.firstinspires.ftc.teamcode.MovementAPI; @Config public class Poser { Hardware hardware; - MovementAPI movement; + AccelLimiter movement; Telemetry telemetry; EncoderIntegrator integrator; + FieldVector lastTargetPos; + double lastTargetYaw; + double speed; public static double POS_STOP_THRESHOLD = 5; public static double YAW_STOP_THRESHOLD = 0.5; - public static int LIFT_STOP_THRESHOLD = 7; + public static int LIFT_STOP_THRESHOLD = 15; public static int SLOWDOWN_THRESHOLD_FACTOR = 40; + public static PIDCoefficients POS_PID_COEFFICIENTS = new PIDCoefficients(5, 0.1, 1); + public static PIDCoefficients YAW_PID_COEFFICIENTS = new PIDCoefficients(1/30., 0, 0); + public static PIDCoefficients LIFT_PID_COEFFICIENTS = new PIDCoefficients(1/300., 0, 0); + public Poser(Hardware hardware, double speed, FieldVector initalPos, double initialYaw) { this.hardware = hardware; - this.movement = new MovementAPI(hardware); + this.movement = new AccelLimiter(new MovementAPI(hardware)); this.speed = speed; this.telemetry = hardware.opMode.telemetry; this.integrator = new EncoderIntegrator(hardware, initalPos, initialYaw, telemetry); + this.lastTargetPos = initalPos; + this.lastTargetYaw = initialYaw; } private Pose getPose() { @@ -45,19 +55,23 @@ public class Poser { } public void goTo(NullablePose targetPose) { - Pose currentPose = this.getPose(); - - this.goToPose(new Pose( - targetPose.pos != null ? targetPose.pos : currentPose.pos, - targetPose.yaw != null ? targetPose.yaw : currentPose.yaw, - targetPose.liftPos != null ? targetPose.liftPos : currentPose.liftPos + this.goTo(new Pose( + targetPose.pos != null ? targetPose.pos : this.lastTargetPos, + targetPose.yaw != null ? targetPose.yaw : this.lastTargetYaw, + targetPose.liftPos != null ? targetPose.liftPos : hardware.getLiftEncoder() )); } - public void goToPose(Pose targetPose) { - int loopsCorrect = 0; + public void goTo(Pose targetPose) { + this.lastTargetPos = targetPose.pos; + this.lastTargetYaw = targetPose.yaw; - while (true) { + int loopsCorrect = 0; + PID posPid = new PID(POS_PID_COEFFICIENTS); +// PID yawPid = new PID(YAW_PID_COEFFICIENTS); +// PID liftPid = new PID(LIFT_PID_COEFFICIENTS); + + while (!Thread.interrupted()) { this.integrator.update(); Pose currentPose = this.getPose(); @@ -76,9 +90,7 @@ public class Poser { loopsCorrect = 0; } - if (loopsCorrect >= 120) { - movement.stop(); - hardware.spool.setPower(0); + if (loopsCorrect >= 25) { break; } @@ -91,9 +103,12 @@ public class Poser { telemetry.addData("yawDiff", yawDiff); telemetry.addData("liftDiff", liftDiff); - FieldVector posPower = posDiff.normalized().mul(ellipticalCurve(posDiff.magnitude().valInMM(), POS_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR)); +// FieldVector posPower = posDiff.normalized().mul(ellipticalCurve(posDiff.magnitude().valInMM(), POS_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR)); double yawPower = ellipticalCurve(yawDiff, YAW_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR); double liftPower = ellipticalCurve(liftDiff, LIFT_STOP_THRESHOLD * SLOWDOWN_THRESHOLD_FACTOR); + FieldVector posPower = posDiff.normalized().mul(posPid.update(posDiff.magnitude().valInFeet())); +// double yawPower = yawPid.update(yawDiff); +// double liftPower = liftPid.update(liftDiff); telemetry.addData("posPower", posPower); telemetry.addData("posXPower", posPower.x); @@ -114,5 +129,8 @@ public class Poser { ); hardware.spool.setPower(liftPower); } + + movement.stop(); + hardware.spool.setPower(0); } }