Kova-Decode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/HardwareCoquett.java at master · DevoltRobotics/Kova-Decode · 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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
package org.firstinspires.ftc.teamcode;

import com.bylazar.telemetry.PanelsTelemetry;
import com.pedropathing.follower.Follower;
import com.qualcomm.hardware.limelightvision.LLResult;
import com.qualcomm.hardware.limelightvision.Limelight3A;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;

import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
import org.firstinspires.ftc.teamcode.subsystem.Intake;
import org.firstinspires.ftc.teamcode.subsystem.Shooter;
import org.firstinspires.ftc.teamcode.subsystem.Turret;

public class HardwareCoquett {

    // Motores principales
    public DcMotor transferMotor;

    public DcMotorEx shooterMotor;
    public DcMotorEx disparadorMotor;
    public DcMotor intakeMotor;

    // Servos
    public CRServo torrettCoquette;
    public CRServo ballUp;
    public CRServo elevacionServo;
    public Servo ballStop;
    public Servo ballAlto;
    public Servo asistencia;
    public Servo luz;
    public DigitalChannel laserInput;

    boolean detected;


    // Follower para manejo de movimiento

    public Follower follower;
    public Limelight3A limelight;

    public Intake intake = new Intake(this);

    public Shooter shooter = new Shooter(this);
    public Turret turret = new Turret(this);
    public final Telemetry panelsTelem = PanelsTelemetry.INSTANCE.getFtcTelemetry();

    public final Alliance alliance;

    public LLResult llResult = null;

    public HardwareCoquett(Alliance alliance) {
        this.alliance = alliance;
    }

    public void init(HardwareMap hardwareMap) {

        // Inicializa el seguidor de trayectorias
        follower = Constants.createFollower(hardwareMap);

        // Motores
        transferMotor = hardwareMap.get(DcMotor.class, "intake");

        shooterMotor = hardwareMap.get(DcMotorEx.class, "shooter");
        disparadorMotor = hardwareMap.get(DcMotorEx.class, "disparador");
        laserInput = hardwareMap.get(DigitalChannel.class, "laser");
        laserInput.setMode(DigitalChannel.Mode.INPUT);

        disparadorMotor.setDirection(DcMotorSimple.Direction.REVERSE);

        intakeMotor =hardwareMap.get(DcMotor.class, "transfer");


        // Servos
        torrettCoquette = hardwareMap.get(CRServo.class, "torreta");
        ballUp = hardwareMap.get(CRServo.class, "subeBolas");
        ballAlto = hardwareMap.get(Servo.class, "alto");
        elevacionServo = hardwareMap.get(CRServo.class,"elevacion");
        ballStop = hardwareMap.get(Servo.class, "paraBolas");
        asistencia = hardwareMap.get(Servo.class, "asistencia");
        luz = hardwareMap.get(Servo.class, "luz");


        // Sensor
        limelight = hardwareMap.get(Limelight3A.class, "limelight");
        asistencia.setDirection(Servo.Direction.REVERSE);

        limelight.start();
        limelight.setPollRateHz(60);

        limelight.pipelineSwitch(0);
        ballStop.setDirection(Servo.Direction.REVERSE);
    }

    public void update() {
        llResult = limelight.getLatestResult();

        boolean stateHigh = laserInput.getState();
        detected = stateHigh;

        shooter.update();
        turret.update();

        panelsTelem.addData("LL isValid", llResult.isValid());
        panelsTelem.addData("LL tA", getAllianceTA());
        panelsTelem.addData("LL tX", getAllianceTX());

        panelsTelem.update();

        follower.update();
    }


    public Double getAllianceTA() {
        if (llResult != null && llResult.isValid() && !llResult.getFiducialResults().isEmpty()) {
            int id = llResult.getFiducialResults().get(0).getFiducialId();

            if (alliance == Alliance.ANY ||
                    (alliance == Alliance.RED && id == 24) ||
                    (alliance == Alliance.BLUE && id == 20)) {
                return llResult.getTa();
            }
        }

        return null;
    }

    public boolean isBallDetected() {
        return detected;
    }


    public Double getAllianceTX() {
        if (llResult != null && llResult.isValid() && !llResult.getFiducialResults().isEmpty()) {
            int id = llResult.getFiducialResults().get(0).getFiducialId();

            if (alliance == Alliance.ANY ||
                    (alliance == Alliance.RED && id == 24) ||
                    (alliance == Alliance.BLUE && id == 20)) {
                return llResult.getTx();
            }
        }

        return null;
    }
}