5002ReefScape/autoAlign.java at main · dragonrobotics/5002ReefScape · 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
package frc.robot;

import org.photonvision.PhotonCamera;

import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.RobotCentric;
import com.ctre.phoenix6.swerve.SwerveRequest;

import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.wpilibj2.command.Command;
import frc.robot.Constants.OperatorConstants;
import frc.robot.subsystems.CommandSwerveDrivetrain;

public class autoAlign extends Command{

    private PhotonCamera camera;
    private CommandSwerveDrivetrain swerveDrive;
    private SwerveRequest.FieldCentric requester;

    private PIDController rotationController;
    private PIDController xPidController;
    private PIDController yPidController;

    private Boolean complete;
    private int ID;


    public autoAlign(CommandSwerveDrivetrain swerveDrive, int ID){
        this.swerveDrive = swerveDrive;
        this.ID = ID;
        addRequirements(swerveDrive);

        camera = new PhotonCamera(OperatorConstants.cameraName);
        requester = new SwerveRequest.FieldCentric();

        rotationController = new PIDController(0.5, 0, 0);
        xPidController = new PIDController(0.5, 0.0, 0.0);
        yPidController = new PIDController(0.5, 0.0, 0.0);
    }

    @Override
    public void initialize() {
        complete = false;
    }

    @Override
    public void execute() {
        var result = camera.getLatestResult();
        boolean hasTargets = result.hasTargets();

        if (hasTargets){
            var target = result.getBestTarget();
            if (target.getFiducialId() == this.ID){
                Transform3d bestCameraToTarget = target.getBestCameraToTarget();
                double distance = OperatorConstants.distanceToTag;


                double rError = bestCameraToTarget.getRotation().getZ();
                double yError = bestCameraToTarget.getY();
                double xError = bestCameraToTarget.getX() - distance;

                double xOutput = xPidController.calculate(xError, 0);
                double yOutput = yPidController.calculate(yError, 0);
                double rOutput = rotationController.calculate(rError, 0);

                swerveDrive.setControl(requester.withVelocityX(xOutput).withVelocityY(yOutput).withRotationalRate(rOutput));
            }
        }
    }

    @Override
    public boolean isFinished(){
        double positionTolerance = 0.02; // 2 cm
        double rotationTolerance = 0.05; // Small angle in radians

        var result = camera.getLatestResult();
        if (result.hasTargets()) {
            var target = result.getBestTarget();
            if (target.getFiducialId() == this.ID) {
                Transform3d bestCameraToTarget = target.getBestCameraToTarget();
                double xError = bestCameraToTarget.getX() - (OperatorConstants.distanceToTag);
                double yError = bestCameraToTarget.getY();
                double rError = bestCameraToTarget.getRotation().getZ();

                return Math.abs(xError) < positionTolerance &&
                       Math.abs(yError) < positionTolerance &&
                       Math.abs(rError) < rotationTolerance;
            }
        }
        return false;
    }

    @Override
    public void end(boolean interrupted) {
        swerveDrive.setControl(requester.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
    }
}