pid stuff
This commit is contained in:
parent
4073d7e951
commit
5501c4bdc6
|
@ -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(),
|
||||
|
|
|
@ -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)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
}
|
|
@ -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;
|
||||
}
|
||||
}
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in a new issue