Vision49/Trackingandspeedcontrol.java at main · Niconut/Vision49 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
package org.firstinspires.ftc.teamcode;

import static java.lang.Math.atan2;

import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.CRServoImplEx;
import com.qualcomm.robotcore.hardware.DcMotorEx;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;

@TeleOp(name="Analog Turret Logic", group="Pinpoint")
public class Trackingandspeedcontrol extends LinearOpMode {

    // Hardware
    private CRServoImplEx turretServo;
    private AnalogInput turretAnalog; // Only source of position data
    private GoBildaPinpointDriver odo;
    private DcMotorEx launcher;

    // Tracking Variables (Must stay outside the loop)
    private double lastLocalRad = 0;
    private int fullRotations = 0;
    private double totalTurretRadians = 0;

    // Constants
    private final double MAX_VOLTAGE = 5.0;
    private final double TWO_PI = 2.0 * Math.PI;
    private final double TARGET_TOTAL_RAD = 5.0 * TWO_PI; // Max range (10*PI)

    @Override
    public void runOpMode() {
        // 1. Hardware Map
        odo = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint");
        turretServo = hardwareMap.get(CRServoImplEx.class, "turretServo");
        turretAnalog = hardwareMap.get(AnalogInput.class, "turretFeedback");
        launcher = hardwareMap.get(DcMotorEx.class, "ScoringShooter");

        // 2. Pinpoint Config (Offsets in MM as per goBILDA standard)
        odo.setOffsets(-3.75, -3.17, DistanceUnit.INCH);
        odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
        odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.REVERSED, GoBildaPinpointDriver.EncoderDirection.FORWARD);
        odo.resetPosAndIMU();

        // Initialize starting position based ONLY on voltage
        lastLocalRad = (turretAnalog.getVoltage() / MAX_VOLTAGE) * TWO_PI;

        waitForStart();

        while (opModeIsActive()) {
            odo.update();

            // --- STEP 1: READ ANALOG VOLTAGE (NO SERVO POSITION USED) ---
            double currentLocalRad = (turretAnalog.getVoltage() / MAX_VOLTAGE) * TWO_PI;

            // Detect rollover (crossing the 0/360 boundary)
            double delta = currentLocalRad - lastLocalRad;
            if (delta < -Math.PI) fullRotations++;      // Clockwise jump
            else if (delta > Math.PI) fullRotations--; // Counter-clockwise jump

            totalTurretRadians = (fullRotations * TWO_PI) + currentLocalRad;
            lastLocalRad = currentLocalRad;

            // --- STEP 2: CALCULATE FIELD TARGET ---7

            Pose2D robotPose = odo.getPosition();
            double y = robotPose.getX(DistanceUnit.INCH);
            double x = robotPose.getY(DistanceUnit.INCH);
            double H = robotPose.getHeading(AngleUnit.RADIANS);
            double robotH = -(H);
            double RED_GOAL_X = 13.63;
            double RED_GOAL_Y = 127.64;

            double dx = RED_GOAL_X - x;
            double dy = RED_GOAL_Y - y;
            double floorDistance = Math.hypot(dx, dy);


            double power = 975 + (floorDistance - 70) * ((1300.0 - 975.0) / (124.0 - 70.0));

            launcher.setVelocity(power);

            // Absolute angle from robot to goal redgoal
           double targetFieldAngle = atan2(127.64 - y, 13.63- x);

            // Relative angle target (where the turret should be compared to the robot front)
            double targetRelativeAngle = targetFieldAngle - robotH;

            // --- STEP 3: DRIVE SERVO VIA POWER ---
            // Calculate error between target and our accumulated analog position
            double error = targetRelativeAngle - totalTurretRadians;

            // Normalize error to the closest rotation so it doesn't spin 5 times to fix 1 degree
            while (error > Math.PI) error -= TWO_PI;
            while (error < -Math.PI) error += TWO_PI;

            // Proportional Gain (Adjust kP to change speed/accuracy)
            double kP = 0.8;
            double pos = error * kP;

            // Safety Clamping
            pos = Math.max(-1.0, Math.min(1.0, pos));

            // Apply power to the CR Servo
            turretServo.setPower(pos);

            // --- TELEMETRY ---
            telemetry.addData("Voltage", turretAnalog.getVoltage());
            telemetry.addData("0.0-1.0 Progress", totalTurretRadians / TARGET_TOTAL_RAD);
            telemetry.addData("Turret Angle (Rad)", totalTurretRadians);
            telemetry.addData("Error", error);
            telemetry.addData("pos y", x);
            telemetry.addData("pos x", y);
            telemetry.addData("heading", robotH);
            telemetry.addData("velocity", power);
            telemetry.update();
        }
    }
}