Skip to content

Commit 16a935b

Browse files
edsoneliedsoneli
committed
how bot this 1
1 parent db2dce9 commit 16a935b

3 files changed

Lines changed: 81 additions & 35 deletions

File tree

‎TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPrismDriver.java‎

Lines changed: 79 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,5 @@
11
package org.firstinspires.ftc.teamcode;
22

3-
43
import com.qualcomm.robotcore.hardware.I2cAddr;
54
import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
65
import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
@@ -10,43 +9,52 @@
109

1110
/**
1211
* GoBilda Prism RGB LED Driver for FTC
13-
*
14-
* WIRING:
15-
* - Plug Prism I²C cable into any I²C port on the REV Control Hub
12+
* Based on official goBILDA user guide (3118-2855-0001)
1613
*
1714
* ROBOT CONFIGURATION (Driver Hub):
18-
* - Add I²C device on the port the Prism is plugged into
1915
* - Device type: "goBILDA Prism LED Driver"
2016
* - Name: prism_led
21-
* - Address: 0x38 <-- THIS IS THE CORRECT ADDRESS (NOT 0x40!)
17+
* - Address: 0x38
2218
*
2319
* USAGE:
2420
* GoBildaPrismDriver prism = hardwareMap.get(GoBildaPrismDriver.class, "prism_led");
25-
* prism.setColor(255, 0, 0); // Red
26-
* prism.setColor(0, 255, 0); // Green
27-
* prism.setColor(0, 0, 255); // Blue
28-
* prism.setColor(255, 255, 255); // White
29-
* prism.setColor(0, 0, 0); // Off
21+
* prism.setColor(255, 0, 0); // Red solid color
22+
* prism.setColor(0, 255, 0); // Green solid color
23+
* prism.setColor(0, 0, 255); // Blue solid color
24+
* prism.setColor(255, 255, 255); // White solid color
25+
* prism.turnOff(); // Off
3026
*/
3127

3228
@I2cDeviceType
3329
@DeviceProperties(
3430
name = "goBILDA Prism LED Driver",
35-
description = "goBILDA Prism RGB LED Driver (I2C, address 0x38)",
31+
description = "goBILDA Prism RGB LED Driver (I2C 0x38)",
3632
xmlTag = "GoBildaPrismDriver"
3733
)
3834
public class GoBildaPrismDriver extends I2cDeviceSynchDevice<I2cDeviceSynch> {
3935

4036
// -------------------------------------------------------------------------
41-
// The goBILDA Prism has its OWN firmware — NOT raw PCA9685.
42-
// Correct I2C address from the official goBILDA product page: 0x38
37+
// Official register map from goBILDA user guide
4338
// -------------------------------------------------------------------------
44-
public static final I2cAddr DEFAULT_ADDRESS = I2cAddr.create7bit(0x38);
39+
public static final I2cAddr DEFAULT_ADDRESS = I2cAddr.create7bit(0x38);
40+
41+
// Control register — write 32-bit value to clear all animations
42+
private static final int REG_CONTROL = 0x06;
43+
private static final int CONTROL_CLEAR_ANIM = (1 << 25); // bit 25 = clear animations
44+
45+
// Layer 0 register (0x08) — this is where we write our animation
46+
private static final int REG_LAYER_0 = 0x08;
47+
48+
// Sub-registers for any layer
49+
private static final int SUB_SELECTED_ANIM = 0x00; // animation type
50+
private static final int SUB_BRIGHTNESS = 0x01; // 0–100
51+
private static final int SUB_START_INDEX = 0x02; // start LED index
52+
private static final int SUB_STOP_INDEX = 0x03; // stop LED index (255 = all)
53+
private static final int SUB_PRIMARY_COLOR = 0x04; // 3 bytes: R, G, B
4554

46-
// Prism I2C register map (from goBILDA official documentation)
47-
private static final int REG_RED = 0x00; // Red channel (0–255)
48-
private static final int REG_GREEN = 0x01; // Green channel (0–255)
49-
private static final int REG_BLUE = 0x02; // Blue channel (0–255)
55+
// Animation type IDs (from official docs)
56+
private static final int ANIM_SOLID_COLOR = 0x01;
57+
private static final int ANIM_NONE = 0x00;
5058

5159
// -------------------------------------------------------------------------
5260
// Constructor
@@ -63,7 +71,7 @@ public GoBildaPrismDriver(I2cDeviceSynch deviceClient, boolean deviceClientIsOwn
6371
// -------------------------------------------------------------------------
6472
@Override
6573
protected synchronized boolean doInitialize() {
66-
setColor(0, 0, 0); // Start with LEDs off
74+
turnOff();
6775
return true;
6876
}
6977

@@ -72,7 +80,7 @@ protected synchronized boolean doInitialize() {
7280
// -------------------------------------------------------------------------
7381

7482
/**
75-
* Set the RGB color of the Prism LED strip.
83+
* Set a solid RGB color across all LEDs.
7684
* @param r Red 0–255
7785
* @param g Green 0–255
7886
* @param b Blue 0–255
@@ -82,14 +90,32 @@ public void setColor(int r, int g, int b) {
8290
g = clamp(g, 0, 255);
8391
b = clamp(b, 0, 255);
8492

85-
// Write all 3 channels in one I2C transaction using auto-increment
86-
byte[] data = { (byte) r, (byte) g, (byte) b };
87-
deviceClient.write(REG_RED, data, I2cWaitControl.NONE);
93+
// Step 1: Select "Solid Color" animation on Layer 0
94+
writeLayerSubReg(REG_LAYER_0, SUB_SELECTED_ANIM, new byte[]{ (byte) ANIM_SOLID_COLOR });
95+
96+
// Step 2: Set brightness to 100%
97+
writeLayerSubReg(REG_LAYER_0, SUB_BRIGHTNESS, new byte[]{ (byte) 100 });
98+
99+
// Step 3: Start at LED 0
100+
writeLayerSubReg(REG_LAYER_0, SUB_START_INDEX, new byte[]{ (byte) 0 });
101+
102+
// Step 4: Stop at LED 255 (all LEDs)
103+
writeLayerSubReg(REG_LAYER_0, SUB_STOP_INDEX, new byte[]{ (byte) 255 });
104+
105+
// Step 5: Write RGB color (3 bytes)
106+
writeLayerSubReg(REG_LAYER_0, SUB_PRIMARY_COLOR, new byte[]{
107+
(byte) r, (byte) g, (byte) b
108+
});
88109
}
89110

90-
/** Turn off all LEDs. */
111+
/**
112+
* Turn off all LEDs by clearing all animations.
113+
*/
91114
public void turnOff() {
92-
setColor(0, 0, 0);
115+
// Write CLEAR_ANIMATIONS bit to the control register
116+
// Control register is 32-bit
117+
byte[] clearCmd = intToBytes(CONTROL_CLEAR_ANIM);
118+
deviceClient.write(REG_CONTROL, clearCmd, I2cWaitControl.NONE);
93119
}
94120

95121
/**
@@ -104,18 +130,38 @@ public void setColorInt(int color) {
104130
// Required overrides
105131
// -------------------------------------------------------------------------
106132
@Override
107-
public Manufacturer getManufacturer() {
108-
return Manufacturer.Other;
109-
}
133+
public Manufacturer getManufacturer() { return Manufacturer.Other; }
110134

111135
@Override
112-
public String getDeviceName() {
113-
return "goBILDA Prism LED Driver";
114-
}
136+
public String getDeviceName() { return "goBILDA Prism LED Driver"; }
115137

116138
// -------------------------------------------------------------------------
117-
// Helper
139+
// Private helpers
118140
// -------------------------------------------------------------------------
141+
142+
/**
143+
* Write to a layer sub-register.
144+
* Protocol: [device_addr, layer_reg, sub_reg, data...]
145+
* We write: [layer_reg, sub_reg, data...] (device addr handled by I2C bus)
146+
*/
147+
private void writeLayerSubReg(int layerReg, int subReg, byte[] data) {
148+
// Build packet: sub-register byte followed by data bytes
149+
byte[] packet = new byte[1 + data.length];
150+
packet[0] = (byte) subReg;
151+
System.arraycopy(data, 0, packet, 1, data.length);
152+
deviceClient.write(layerReg, packet, I2cWaitControl.NONE);
153+
}
154+
155+
/** Convert 32-bit int to 4 bytes, little-endian */
156+
private byte[] intToBytes(int value) {
157+
return new byte[]{
158+
(byte) (value & 0xFF),
159+
(byte) ((value >> 8) & 0xFF),
160+
(byte) ((value >> 16) & 0xFF),
161+
(byte) ((value >> 24) & 0xFF)
162+
};
163+
}
164+
119165
private int clamp(int value, int min, int max) {
120166
return Math.max(min, Math.min(max, value));
121167
}

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@ public class full_bot_w_load extends OpMode {
2626
private Servo claw;
2727
private Servo loop;
2828
// Constants
29-
private static final int TARGET_FLYWHEEL = 5090; // target ticks/sec
29+
private static final int TARGET_FLYWHEEL = 9999999; // target ticks/sec
3030
private static final int RAMP_RATE = 900; // smaller step = smoother ramp
3131

3232
// Servo position constants

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,7 +44,7 @@ private enum LedMode { AUTO, RED, GREEN, BLUE, WHITE, OFF }
4444
private Servo loop;
4545

4646
// Constants
47-
private static final int TARGET_FLYWHEEL = 5090;
47+
private static final int TARGET_FLYWHEEL = 999999;
4848
private static final int RAMP_RATE = 900;
4949
private static final double CLAW_CLOSED = 1.0;
5050
private static final double CLAW_OPEN = 0.25;

0 commit comments

Comments
 (0)