power-play/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/kinematics/Poser.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;
    Dampener 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 Dampener(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);
    }
}