Skip to content

Commit 06aae11

Browse files
edsoneliedsoneli
committed
the god 1
1 parent 1b44de0 commit 06aae11

1 file changed

Lines changed: 54 additions & 158 deletions

File tree

  • TeamCode/src/main/java/org/mrpsvt/capital_robotics/teleop

‎TeamCode/src/main/java/org/mrpsvt/capital_robotics/teleop/lights.java‎

Lines changed: 54 additions & 158 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,5 @@
11
package org.mrpsvt.capital_robotics.teleop;
22

3-
43
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
54
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
65
import com.qualcomm.robotcore.hardware.DcMotor;
@@ -20,33 +19,13 @@
2019
public class lights extends OpMode {
2120

2221
// -------------------------------------------------------------------------
23-
// GoBilda Prism RGB LED Driver (PCA9685 over I²C)
22+
// LED
2423
// -------------------------------------------------------------------------
25-
// The GoBilda Prism I²C driver is a PCA9685 PWM controller.
26-
// I²C address: 0x40 (default). Register layout per channel (4 bytes each):
27-
// LEDn_ON_L = base + 0
28-
// LEDn_ON_H = base + 1
29-
// LEDn_OFF_L = base + 2
30-
// LEDn_OFF_H = base + 3
31-
// Channel mapping (GoBilda Prism, 4 strips):
32-
// CH0 = Red (all strips tied together on this driver)
33-
// CH1 = Green
34-
// CH2 = Blue
35-
// We write a 12-bit PWM OFF count (0–4095) to set brightness per channel.
36-
// MODE1 register (0x00) must be written to wake the device from sleep.
37-
38-
private static final byte PCA9685_MODE1 = 0x00;
39-
private static final byte PCA9685_LED0_ON_L = 0x06; // CH0 base
40-
private static final byte PCA9685_LED1_ON_L = 0x0A; // CH1 base
41-
private static final byte PCA9685_LED2_ON_L = 0x0E; // CH2 base
42-
private static final int PCA9685_MAX_PWM = 4095;
43-
4424
private GoBildaPrismDriver ledDriver;
45-
private boolean ledsEnabled = true;
46-
private boolean lastButtonB = false; // debounce for LED toggle (gp2 B)
47-
private boolean lastButtonX = false; // debounce for color cycle (gp2 X)
25+
private boolean ledsEnabled = true;
26+
private boolean lastButtonB = false;
27+
private boolean lastButtonX = false;
4828

49-
// Manual color mode cycling
5029
private enum LedMode { AUTO, RED, GREEN, BLUE, WHITE, OFF }
5130
private LedMode ledMode = LedMode.AUTO;
5231
private static final LedMode[] LED_CYCLE = {
@@ -56,56 +35,45 @@ private enum LedMode { AUTO, RED, GREEN, BLUE, WHITE, OFF }
5635
private int ledModeIndex = 0;
5736

5837
// -------------------------------------------------------------------------
59-
// Flywheels
38+
// Motors & Servos
6039
// -------------------------------------------------------------------------
6140
private DcMotorEx flywheel;
6241
private DcMotorEx flywheel2;
6342
private DcMotorEx lodewheel;
64-
65-
// Servos
6643
private Servo claw;
6744
private Servo loop;
6845

6946
// Constants
70-
private static final int TARGET_FLYWHEEL = 5090; // target ticks/sec
71-
private static final int RAMP_RATE = 900; // smaller step = smoother ramp
47+
private static final int TARGET_FLYWHEEL = 5090;
48+
private static final int RAMP_RATE = 900;
49+
private static final double CLAW_CLOSED = 1.0;
50+
private static final double CLAW_OPEN = 0.25;
51+
private static final double LOOP_CLOSE = 5.0;
52+
private static final double BOB = 6000;
7253

73-
// Servo position constants
74-
private static final double CLAW_CLOSED = 1.0;
75-
private static final double CLAW_OPEN = 0.25;
76-
private static final double loop_close = 5;
77-
78-
// Track flywheel velocity for ramping
7954
private double currentFlywheelVelocity = 0;
8055
private double currentLinearPosition = 0.5;
8156

82-
private final double bob = 6000;
83-
84-
// Button state tracking for debouncing (existing)
8557
private boolean lastLeftBumper = false;
8658
private boolean lastRightBumper = false;
8759

88-
// Speed control variables
8960
private float forwardSpeed = 0.5f;
9061
private float strafeSpeed = 0.5f;
9162
private float turnSpeed = 0.5f;
9263

93-
private ControlMap controls;
94-
private MecanumDrive drive;
64+
private ControlMap controls;
65+
private MecanumDrive drive;
9566
private DriveConstants driveConstants;
96-
private RevIMU imu;
67+
private RevIMU imu;
9768

9869
// =========================================================================
9970
// INIT
10071
// =========================================================================
10172
@Override
10273
public void init() {
10374
// --- LED Driver ---
104-
// In your robot configuration, add an "I2C Device (Synch)" named "prism_led"
105-
// on the I²C bus connected to the GoBilda Prism driver (default address 0x40).
10675
ledDriver = hardwareMap.get(GoBildaPrismDriver.class, "prism_led");
107-
initPCA9685();
108-
setLedColor(0, 255, 0); // Start green (idle)
76+
ledDriver.setColor(0, 255, 0); // Start green (idle)
10977

11078
// --- Flywheel motors ---
11179
flywheel = hardwareMap.get(DcMotorEx.class, "flywheel");
@@ -114,29 +82,25 @@ public void init() {
11482
flywheel.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
11583
flywheel2.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
11684
lodewheel.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
117-
118-
// Flip one flywheel so they counter-rotate
11985
flywheel.setDirection(DcMotorEx.Direction.FORWARD);
12086
flywheel2.setDirection(DcMotorEx.Direction.REVERSE);
12187

12288
// --- Servos ---
12389
claw = hardwareMap.get(Servo.class, "claw");
12490
claw.setPosition(CLAW_CLOSED);
12591
loop = hardwareMap.get(Servo.class, "loop");
126-
loop.setPosition(loop_close);
92+
loop.setPosition(LOOP_CLOSE);
12793

12894
// --- Drive system ---
12995
DriveBase driveBase = new DriveBase(hardwareMap);
130-
controls = new ControlMap(gamepad1, gamepad2);
131-
drive = driveBase.mecanum;
132-
imu = driveBase.imu;
96+
controls = new ControlMap(gamepad1, gamepad2);
97+
drive = driveBase.mecanum;
98+
imu = driveBase.imu;
13399
driveConstants = new DriveConstants();
134100

135-
telemetry.addData("Status", "Initialized");
136-
telemetry.addData("LED", "I2C Prism driver ready");
137-
telemetry.addData("Controls", "Gp2 X = cycle color mode | Gp2 B = toggle LEDs");
138-
telemetry.addData("Controls", "A=Flywheel, LB=Retract, RB=Extend");
139-
telemetry.addData("Controls", "LT=Close Claw, RT=Open Claw");
101+
telemetry.addData("Status", "Initialized");
102+
telemetry.addData("Controls", "Gp2 X = cycle color | Gp2 B = toggle LEDs");
103+
telemetry.addData("Controls", "A=Flywheel | LT=Close Claw | RT=Open Claw");
140104
telemetry.update();
141105
}
142106

@@ -177,14 +141,15 @@ public void loop() {
177141
if (gamepad2.a) {
178142
currentFlywheelVelocity = Math.min(currentFlywheelVelocity + RAMP_RATE, TARGET_FLYWHEEL);
179143
} else {
144+
// FIX 1: Flywheels ramp DOWN when A is released instead of staying on
180145
currentFlywheelVelocity = Math.max(currentFlywheelVelocity - RAMP_RATE, 0);
181146
}
182147
flywheel.setVelocity(currentFlywheelVelocity);
183148
flywheel2.setVelocity(currentFlywheelVelocity);
184149

185150
// --- Intake ---
186151
if (gamepad2.dpad_down) {
187-
lodewheel.setVelocity(bob);
152+
lodewheel.setVelocity(BOB);
188153
} else {
189154
lodewheel.setVelocity(0);
190155
}
@@ -200,46 +165,44 @@ public void loop() {
200165
if (gamepad2.y) {
201166
loop.setPosition(0);
202167
} else if (gamepad2.dpad_right) {
203-
loop.setPosition(5);
168+
loop.setPosition(LOOP_CLOSE);
204169
} else if (gamepad2.dpad_left) {
205170
loop.setPosition(0.5);
206171
}
207172

208-
// --- LED: toggle on/off (Gamepad 2 B, debounced) ---
173+
// --- LED toggle on/off (Gp2 B, debounced) ---
209174
if (gamepad2.b && !lastButtonB) {
210175
ledsEnabled = !ledsEnabled;
211176
if (!ledsEnabled) {
212-
setLedColor(0, 0, 0); // hard off
177+
ledDriver.setColor(0, 0, 0); // hard off
213178
}
214179
}
215180
lastButtonB = gamepad2.b;
216181

217-
// --- LED: cycle color mode (Gamepad 2 X, debounced) ---
182+
// --- LED cycle color mode (Gp2 X, debounced) ---
218183
if (gamepad2.x && !lastButtonX) {
219184
ledModeIndex = (ledModeIndex + 1) % LED_CYCLE.length;
220185
ledMode = LED_CYCLE[ledModeIndex];
221186
}
222187
lastButtonX = gamepad2.x;
223188

224-
// --- LED: update color based on mode ---
189+
// --- LED update ---
225190
if (ledsEnabled) {
226191
updateLeds();
227192
}
228193

229-
// --- Existing debounce state ---
230194
lastLeftBumper = gamepad2.left_bumper;
231195
lastRightBumper = gamepad2.right_bumper;
232196

233197
// --- Telemetry ---
234-
telemetry.addData("Status", "Running");
235-
telemetry.addData("LED Mode", ledMode.name());
236-
telemetry.addData("LEDs Enabled", ledsEnabled);
237-
telemetry.addData("Flywheel Target", TARGET_FLYWHEEL);
238-
telemetry.addData("Flywheel Commanded", currentFlywheelVelocity);
239-
telemetry.addData("Flywheel1 Actual", flywheel.getVelocity());
240-
telemetry.addData("Flywheel2 Actual", flywheel2.getVelocity());
241-
telemetry.addData("Linear Servo", "%.2f", currentLinearPosition);
242-
telemetry.addData("Claw Position", "%.2f", claw.getPosition());
198+
telemetry.addData("Status", "Running");
199+
telemetry.addData("LED Mode", ledMode.name());
200+
telemetry.addData("LEDs Enabled", ledsEnabled);
201+
telemetry.addData("Flywheel Target", TARGET_FLYWHEEL);
202+
telemetry.addData("Flywheel Commanded", currentFlywheelVelocity);
203+
telemetry.addData("Flywheel1 Actual", flywheel.getVelocity());
204+
telemetry.addData("Flywheel2 Actual", flywheel2.getVelocity());
205+
telemetry.addData("Claw Position", "%.2f", claw.getPosition());
243206
telemetry.update();
244207
}
245208

@@ -248,98 +211,31 @@ public void loop() {
248211
// =========================================================================
249212

250213
/**
251-
* Decide what color to show based on the current ledMode.
252-
* In AUTO mode, LED color reflects robot state:
253-
* - Flywheel spinning → Blue (ready to launch)
254-
* - Claw closed → Red (holding game element)
255-
* - Idle → Green
214+
* AUTO mode:
215+
* Flywheel spinning → Blue (ready to launch)
216+
* Loop closed → Red (loop engaged)
217+
* Claw closed → Red (holding element)
218+
* Idle → Green
256219
*/
257220
private void updateLeds() {
258221
switch (ledMode) {
259222
case AUTO:
260223
if (currentFlywheelVelocity > 500) {
261-
// Flywheel active: scale blue with speed
262-
int blue = (int) ((currentFlywheelVelocity = 2580) * 255);
263-
setLedColor(0, 255, 0);
264-
} else if (loop.getPosition() >= loop_close - 0.05) {
265-
setLedColor(0, 0, 255); // Claw closed: red
266-
} else if (claw.getPosition() >= CLAW_CLOSED -0.05) {
267-
setLedColor(0,0,255);
224+
// FIX 2: Was "currentFlywheelVelocity = 2580" (assignment bug), now just sets blue
225+
ledDriver.setColor(0, 0, 255); // Blue = flywheel spinning
226+
} else if (loop.getPosition() >= LOOP_CLOSE - 0.05) {
227+
ledDriver.setColor(255, 0, 0); // Red = loop closed
228+
} else if (claw.getPosition() >= CLAW_CLOSED - 0.05) {
229+
ledDriver.setColor(255, 0, 0); // Red = claw closed
268230
} else {
269-
setLedColor(255, 0, 0); // Idle: green
231+
ledDriver.setColor(0, 255, 0); // Green = idle
270232
}
271233
break;
272-
case RED:
273-
setLedColor(255, 0, 0);
274-
break;
275-
case GREEN:
276-
setLedColor(0, 255, 0);
277-
break;
278-
case BLUE:
279-
setLedColor(0, 0, 255);
280-
break;
281-
case WHITE:
282-
setLedColor(255, 255, 255);
283-
break;
284-
case OFF:
285-
setLedColor(0, 0, 0);
286-
break;
234+
case RED: ledDriver.setColor(255, 0, 0); break;
235+
case GREEN: ledDriver.setColor(0, 255, 0); break;
236+
case BLUE: ledDriver.setColor(0, 0, 255); break;
237+
case WHITE: ledDriver.setColor(255, 255, 255); break;
238+
case OFF: ledDriver.setColor(0, 0, 0); break;
287239
}
288240
}
289-
290-
/**
291-
* Wake the PCA9685 from sleep by clearing the SLEEP bit in MODE1.
292-
* Also set the prescaler for ~1 kHz PWM (optional but good practice).
293-
*/
294-
private void initPCA9685() {
295-
// Clear sleep bit (bit 4) to enable oscillator
296-
// 0x00 = normal mode, auto-increment enabled (bit 5 set → 0x20)
297-
// AI + normal mode
298-
// Allow oscillator to stabilize
299-
try { Thread.sleep(10); } catch (InterruptedException ignored) {}
300-
}
301-
302-
/**
303-
* Write an RGB color (0–255 per channel) to the GoBilda Prism driver.
304-
* Converts 8-bit channel values to 12-bit PCA9685 OFF counts.
305-
*
306-
* @param r Red 0–255
307-
* @param g Green 0–255
308-
* @param b Blue 0–255
309-
*/
310-
private void setLedColor(int r, int g, int b) {
311-
int rPwm = scale8to12(r);
312-
int gPwm = scale8to12(g);
313-
int bPwm = scale8to12(b);
314-
315-
// CH0 = Red, CH1 = Green, CH2 = Blue
316-
writePwmChannel(PCA9685_LED0_ON_L, rPwm);
317-
writePwmChannel(PCA9685_LED1_ON_L, gPwm);
318-
writePwmChannel(PCA9685_LED2_ON_L, bPwm);
319-
}
320-
321-
/**
322-
* Write a 12-bit PWM value to one PCA9685 channel.
323-
* ON count is always 0 (phase start at tick 0).
324-
* OFF count = pwmValue (the tick at which the signal goes low).
325-
*
326-
* Register layout for each channel (4 bytes at channelBase):
327-
* [0] LEDn_ON_L (bits 0–7 of ON count)
328-
* [1] LEDn_ON_H (bits 8–11 of ON count)
329-
* [2] LEDn_OFF_L (bits 0–7 of OFF count)
330-
* [3] LEDn_OFF_H (bits 8–11 of OFF count)
331-
*/
332-
private void writePwmChannel(byte channelBase, int pwmValue) {
333-
byte[] data = new byte[4];
334-
data[0] = 0x00; // ON_L = 0
335-
data[1] = 0x00; // ON_H = 0
336-
data[2] = (byte) (pwmValue & 0xFF); // OFF_L = low byte
337-
data[3] = (byte) ((pwmValue >> 8) & 0x0F); // OFF_H = high nibble (12-bit)
338-
339-
}
340-
341-
/** Scale an 8-bit brightness value (0–255) to a 12-bit PCA9685 value (0–4095). */
342-
private int scale8to12(int value8bit) {
343-
return (value8bit * PCA9685_MAX_PWM) / 255;
344-
}
345241
}

0 commit comments

Comments
 (0)