power-play/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/Gamepad.java

package org.firstinspires.ftc.teamcode.teleop;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.teamcode.Hardware;
import org.firstinspires.ftc.teamcode.MotorPowers;
import org.firstinspires.ftc.teamcode.MovementAPI;
import org.firstinspires.ftc.teamcode.Strings;
import org.firstinspires.ftc.teamcode.kinematics.LiftPosition;
import org.firstinspires.ftc.teamcode.kinematics.Poser;

@TeleOp(name = Strings.dualGamepadOpModeName)
public class Gamepad extends OpMode {
    Hardware hardware;
    MovementAPI movement;
    double speed = 0.6;
    LiftPosition targetLiftPos = null;

    @Override
    public void init() {
        hardware = new Hardware(this);
        movement = new MovementAPI(hardware);

        hardware.setClawPosition(1);
        hardware.wrist.setPosition(0.5);
    }

    @Override
    public void loop() {
        if (gamepad1.left_bumper) {
            speed -= 0.005;
        }

        if (gamepad1.right_bumper) {
            speed += 0.005;
        }

        speed = Math.max(0, Math.min(speed, 1));

        if (gamepad2.left_stick_y != 0 || gamepad2.left_stick_button) {
            targetLiftPos = null;
        } else if (gamepad2.a) {
            targetLiftPos = LiftPosition.HIGH;
        } else if (gamepad2.b) {
            targetLiftPos = LiftPosition.FLOOR;
        }

        int spoolPos = hardware.getLiftEncoder();

        double pow;
        if (targetLiftPos != null) {
            int spoolPosDiff = targetLiftPos.getEncoderVal() - spoolPos;
            pow = Poser.ellipticalCurve(spoolPosDiff, Poser.LIFT_STOP_THRESHOLD * Poser.SLOWDOWN_THRESHOLD_FACTOR);
        } else {
            pow = -gamepad2.left_stick_y;
        }

        if (gamepad2.y) {
            telemetry.addLine("lift status: !!! override set !!!");
        } else {
            if (spoolPos < -50) {
                telemetry.addLine("lift status: too far below min, moving up");
                pow = Math.max(pow, 0.2);
            } else if (spoolPos < 0) {
                telemetry.addLine("lift status: below min, stopping downward motion");
                pow = Math.max(pow, 0);
            } else if (spoolPos < 100) {
                telemetry.addLine("lift status: close to min, slowing downward motion");
                pow = Math.max(pow, -0.5);
            } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL + 50) {
                telemetry.addLine("lift status: too far above max, moving down");
                pow = Math.min(pow, -0.2);
            } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL) {
                telemetry.addLine("lift status: above max, stopping upward motion");
                pow = Math.min(pow, 0);
            } else if (spoolPos > LiftPosition.HIGH_ENCODER_VAL - 100) {
                telemetry.addLine("lift status: close to max, slowing upward motion");
                pow = Math.min(pow, 0.5);
            } else {
                telemetry.addLine("lift status: ok");
            }
        }

        telemetry.addLine("lift power: " + pow);
        hardware.spool.setPower(pow);

        hardware.setClawPosition(1 - gamepad2.left_trigger);
        telemetry.addLine("wrist pos: " + gamepad2.right_stick_y);
        hardware.setWristPosition(gamepad2.right_stick_y);

        MotorPowers powers = MotorPowers.calculate(
                gamepad1.left_stick_x,
                -gamepad1.left_stick_y,
                gamepad1.right_stick_x,
                speed
        );

        telemetry.addLine("fl = " + powers.fl);
        telemetry.addLine("fr = " + powers.fr);
        telemetry.addLine("bl = " + powers.bl);
        telemetry.addLine("br = " + powers.br);

        movement.move(powers);

        telemetry.addLine("speed = " + speed);
        telemetry.addLine("fl encoder: " + hardware.fl.getCurrentPosition());
        telemetry.addLine("fr encoder: " + hardware.fr.getCurrentPosition());
        telemetry.addLine("bl encoder: " + hardware.bl.getCurrentPosition());
        telemetry.addLine("br encoder: " + hardware.br.getCurrentPosition());

        telemetry.addLine("lift encoder = " + spoolPos);
        telemetry.addLine("target lift position = " + (targetLiftPos == null ? "none" : targetLiftPos.toString()));
    }
}