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

pid stuff

Browse source
This commit is contained in:
missing 2023-01-25 23:27:08 -06:00
parent 4073d7e951
commit 5501c4bdc6
5 changed files with 143 additions and 29 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

13
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DashboardTelemetryWrapper.java
View file

@ -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(),

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auto/Test.java
View file

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

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

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

41
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/PID.java Normal file
View file

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

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

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