VCSCdecode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ExampleTeleOp.java at master · VCSilverCircuits/VCSCdecode · 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
package org.firstinspires.ftc.teamcode;

import com.bylazar.configurables.annotations.Configurable;
import com.bylazar.telemetry.PanelsTelemetry;
import com.bylazar.telemetry.TelemetryManager;

import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.Pose;


import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;

import org.firstinspires.ftc.teamcode.pedroPathing.Constants;

    @Configurable
    @TeleOp(name = "TeleOp")
    public class ExampleTeleOp extends OpMode {


        // ---------------------------- ROBOT OBJECTS ----------------------------
        private Follower follower;
        private TelemetryManager telemetryM;

        private DcMotor Intake;
        private DcMotor Launcher;

        private DcMotor flywheel1;
        private DcMotor flywheel2;

        private DualMotor flywheel;

        private boolean intakeToggle = false;
        private boolean lastRightTriggerPressed = false;

        boolean lToggle = false;
        boolean lLastTriggeredPressed = false;
        boolean LtriggeredPressed;
        @Override
        public void init() {

            // Pedro Follower
            follower = Constants.createFollower(hardwareMap);
            follower.setStartingPose(new Pose(22.54, 125.169, 143));
            follower.update();

            telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();

            // ---------------------------- FLYWHEEL MOTORS ----------------------------
            flywheel1 = hardwareMap.get(DcMotorEx.class, "flywheel1");
            flywheel2 = hardwareMap.get(DcMotorEx.class, "flywheel2");

            flywheel1.setDirection(DcMotorSimple.Direction.REVERSE);
            flywheel2.setDirection(DcMotorSimple.Direction.FORWARD);

            flywheel1.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
            flywheel2.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);

            // Dual motor wrapper (now valid!)
            flywheel = new DualMotor(flywheel1, flywheel2);

            // ---------------------------- OTHER MECHANISMS ----------------------------
            Intake = hardwareMap.get(DcMotorEx.class, "Intake");
            Launcher = hardwareMap.get(DcMotorEx.class, "Launcher");
        }

//        @Override
//        public void init() {
//
//            // Pedro Follower
//            follower = Constants.createFollower(hardwareMap);
//            follower.setStartingPose(new Pose(22.54, 125.169, 143));
//            follower.update();
//            flywheel1.setDirection(DcMotorSimple.Direction.REVERSE);
//            telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
//            flywheel = new DualMotor(flywheel1,flywheel2);
//
//
//            // Mechanisms
//            Intake = hardwareMap.get(DcMotorEx.class, "Intake");
//            Launcher = hardwareMap.get(DcMotorEx.class, "Launcher");
//
//            // ---------------------------- FLYWHEEL CONTROLLER ----------------------------
//
//        }
        @Override
        public void start() {
            follower.startTeleopDrive();
        }

        // ---------------------------- SCALING FUNCTION ----------------------------
        @Override
        public void loop() {
            follower.update();
            telemetryM.update();

            // ---------------- INTake toggle (Right Trigger) ----------------
            boolean rtPressed = gamepad1.right_trigger > 0.5;
            if (rtPressed && !lastRightTriggerPressed) intakeToggle = !intakeToggle;
            lastRightTriggerPressed = rtPressed;
            Intake.setPower(intakeToggle ? -1 : 0);

            // ---------------- FLYwheel toggle (Left Trigger) ----------------
            LtriggeredPressed = gamepad1.left_trigger > 0.5;
            if (LtriggeredPressed && !lLastTriggeredPressed) {
                lToggle = !lToggle;
            }
            flywheel.setPower(lToggle ? 0.65 : 0.0);
            lLastTriggeredPressed = LtriggeredPressed;

            // ---------------- Manual Launcher ----------------
            Launcher.setPower(gamepad1.left_bumper ? -1 : 0);

            if (gamepad1.dpad_down) {
                Intake.setPower(1);
            }

            // ---------------- Drive ----------------
            follower.setTeleOpDrive(
                -gamepad1.left_stick_y,
                -gamepad1.left_stick_x,
                -gamepad1.right_stick_x * 0.5,
                true
            );
        }

    }