169 lines
6.4 KiB
Java
169 lines
6.4 KiB
Java
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;
|
|
|
|
@Config
|
|
public class Poser {
|
|
Hardware hardware;
|
|
AccelLimiter movement;
|
|
Telemetry telemetry;
|
|
EncoderIntegrator integrator;
|
|
|
|
FieldVector lastTargetPos;
|
|
double lastTargetYaw;
|
|
int lastTargetLiftPos;
|
|
|
|
double speed;
|
|
|
|
public static double POS_STOP_THRESHOLD = 5;
|
|
public static double YAW_STOP_THRESHOLD = 0.5;
|
|
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;
|
|
hardware.initIMU();
|
|
this.movement = new AccelLimiter(hardware);
|
|
this.speed = speed;
|
|
this.telemetry = hardware.opMode.telemetry;
|
|
this.integrator = new EncoderIntegrator(hardware, initalPos, initialYaw, telemetry);
|
|
this.lastTargetPos = initalPos;
|
|
this.lastTargetYaw = initialYaw;
|
|
this.lastTargetLiftPos = hardware.getLiftEncoder();
|
|
}
|
|
|
|
private Pose getPose() {
|
|
return new Pose(
|
|
integrator.pos,
|
|
integrator.imuYaw,
|
|
hardware.getLiftEncoder()
|
|
);
|
|
}
|
|
|
|
public static double ellipticalCurve(double x, double slowdownThreshold) {
|
|
if (Math.abs(x) > slowdownThreshold) {
|
|
return Math.signum(x);
|
|
} else {
|
|
return Math.signum(x) * Math.sqrt(1 - Math.pow((Math.abs(x) / slowdownThreshold) - 1, 2));
|
|
}
|
|
}
|
|
|
|
public void goTo(FieldVector pos, double yaw, int liftPos) {
|
|
this.goTo(new Pose(pos, yaw, liftPos));
|
|
}
|
|
public void goTo(FieldVector pos, double yaw, LiftPosition liftPos) {
|
|
this.goTo(new Pose(pos, yaw, liftPos.getEncoderVal()));
|
|
}
|
|
public void goTo(FieldVector pos, double yaw) {
|
|
this.goTo(new Pose(pos, yaw, this.lastTargetLiftPos));
|
|
}
|
|
public void goTo(double yaw, int liftPos) {
|
|
this.goTo(new Pose(this.lastTargetPos, yaw, liftPos));
|
|
}
|
|
public void goTo(double yaw, LiftPosition liftPos) {
|
|
this.goTo(new Pose(this.lastTargetPos, yaw, liftPos.getEncoderVal()));
|
|
}
|
|
public void goTo(FieldVector pos, int liftPos) {
|
|
this.goTo(new Pose(pos, this.lastTargetYaw, liftPos));
|
|
}
|
|
public void goTo(FieldVector pos, LiftPosition liftPos) {
|
|
this.goTo(new Pose(pos, this.lastTargetYaw, liftPos.getEncoderVal()));
|
|
}
|
|
public void goTo(FieldVector pos) {
|
|
this.goTo(new Pose(pos, this.lastTargetYaw, this.lastTargetLiftPos));
|
|
}
|
|
public void goTo(double yaw) {
|
|
this.goTo(new Pose(this.lastTargetPos, yaw, this.lastTargetLiftPos));
|
|
}
|
|
public void goTo(int liftPos) {
|
|
this.goTo(new Pose(this.lastTargetPos, this.lastTargetYaw, liftPos));
|
|
}
|
|
public void goTo(LiftPosition liftPos) {
|
|
this.goTo(new Pose(this.lastTargetPos, this.lastTargetYaw, liftPos.getEncoderVal()));
|
|
}
|
|
public void goTo() {
|
|
this.goTo(new Pose(this.lastTargetPos, this.lastTargetYaw, this.lastTargetLiftPos));
|
|
}
|
|
|
|
public void goTo(Pose targetPose) {
|
|
this.lastTargetPos = targetPose.pos;
|
|
this.lastTargetYaw = targetPose.yaw;
|
|
this.lastTargetLiftPos = targetPose.liftPos;
|
|
|
|
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();
|
|
|
|
FieldVector posDiff = targetPose.pos.sub(currentPose.pos);
|
|
double yawDiff = targetPose.yaw - currentPose.yaw;
|
|
int liftDiff = targetPose.liftPos - currentPose.liftPos;
|
|
|
|
if (
|
|
Math.abs(posDiff.magnitude().valInMM()) < POS_STOP_THRESHOLD
|
|
&& Math.abs(yawDiff) < YAW_STOP_THRESHOLD
|
|
&& Math.abs(liftDiff) < LIFT_STOP_THRESHOLD
|
|
) {
|
|
loopsCorrect += 1;
|
|
} else {
|
|
loopsCorrect = 0;
|
|
}
|
|
|
|
if (loopsCorrect >= 25) {
|
|
break;
|
|
}
|
|
|
|
telemetry.addData("posX", currentPose.pos.x);
|
|
telemetry.addData("posY", currentPose.pos.y);
|
|
|
|
telemetry.addData("posDiff", posDiff);
|
|
telemetry.addData("posXDiff", posDiff.x);
|
|
telemetry.addData("posYDiff", posDiff.y);
|
|
telemetry.addData("yawDiff", yawDiff);
|
|
telemetry.addData("liftDiff", liftDiff);
|
|
|
|
// 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);
|
|
telemetry.addData("posYPower", posPower.y);
|
|
telemetry.addData("yawPower", yawPower);
|
|
telemetry.addData("liftPower", liftPower);
|
|
telemetry.addData("loopsCorrect", loopsCorrect);
|
|
hardware.dashboardTelemetry.drawTarget(targetPose, currentPose);
|
|
hardware.dashboardTelemetry.drawRobot(currentPose.pos, currentPose.yaw);
|
|
telemetry.update();
|
|
|
|
posPower = posPower.rot(-currentPose.yaw);
|
|
movement.move(
|
|
posPower.x.valInDefaultUnits(),
|
|
posPower.y.valInDefaultUnits(),
|
|
yawPower,
|
|
speed
|
|
);
|
|
hardware.spool.setPower(liftPower);
|
|
}
|
|
|
|
movement.stop();
|
|
hardware.spool.setPower(0);
|
|
}
|
|
}
|