Skip to content

Commit 281dfcc

Browse files
edsoneliedsoneli
committed
to make the lights lite
1 parent af3205a commit 281dfcc

File tree

7 files changed

+774
-105
lines changed

7 files changed

+774
-105
lines changed

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

Lines changed: 125 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,125 @@
1+
package org.firstinspires.ftc.teamcode;
2+
3+
import com.qualcomm.robotcore.hardware.I2cAddr;
4+
import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
5+
import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
6+
import com.qualcomm.robotcore.hardware.I2cWaitControl;
7+
import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
8+
import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
9+
10+
/**
11+
* GoBilda Prism RGB LED Driver for FTC
12+
*
13+
* WIRING:
14+
* - Plug the Prism I²C cable into any I²C port on the REV Control Hub
15+
* - Default I²C address: 0x40
16+
*
17+
* ROBOT CONFIGURATION (Driver Hub):
18+
* - Add an I²C device on the port the Prism is plugged into
19+
* - Select device type: "goBILDA Prism LED Driver"
20+
* - Name it: prism_led
21+
* - Address: 0x40
22+
*
23+
* USAGE IN OPMODE:
24+
* 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
30+
*/
31+
32+
@I2cDeviceType
33+
@DeviceProperties(
34+
name = "goBILDA Prism LED Driver",
35+
description = "goBILDA Prism RGB LED Driver (PCA9685 I2C)",
36+
xmlTag = "GoBildaPrismDriver"
37+
)
38+
public class GoBildaPrismDriver extends I2cDeviceSynchDevice<I2cDeviceSynch> {
39+
40+
// PCA9685 registers
41+
private static final int REG_MODE1 = 0x00;
42+
private static final int REG_MODE2 = 0x01;
43+
private static final int REG_LED0_ON_L = 0x06; // Red
44+
private static final int REG_LED1_ON_L = 0x0A; // Green
45+
private static final int REG_LED2_ON_L = 0x0E; // Blue
46+
private static final int MODE1_AI = 0x20; // Auto-increment
47+
private static final int MAX_PWM = 4095;
48+
49+
public static final I2cAddr DEFAULT_ADDRESS = I2cAddr.create7bit(0x40);
50+
51+
// FIX 1: Only ONE constructor (the duplicate was causing a compile error)
52+
public GoBildaPrismDriver(I2cDeviceSynch deviceClient, boolean deviceClientIsOwned) {
53+
super(deviceClient, deviceClientIsOwned);
54+
this.deviceClient.setI2cAddress(DEFAULT_ADDRESS);
55+
super.registerArmingStateCallback(false);
56+
this.deviceClient.engage();
57+
}
58+
59+
@Override
60+
protected synchronized boolean doInitialize() {
61+
write8(REG_MODE1, MODE1_AI);
62+
try { Thread.sleep(10); } catch (InterruptedException ignored) {}
63+
write8(REG_MODE2, 0x04);
64+
setColor(0, 0, 0);
65+
return true;
66+
}
67+
68+
// -------------------------------------------------------------------------
69+
// Public API
70+
// -------------------------------------------------------------------------
71+
72+
public void setColor(int r, int g, int b) {
73+
writeChannel(REG_LED0_ON_L, scale(clamp(r, 0, 255)));
74+
writeChannel(REG_LED1_ON_L, scale(clamp(g, 0, 255)));
75+
writeChannel(REG_LED2_ON_L, scale(clamp(b, 0, 255)));
76+
}
77+
78+
public void turnOff() {
79+
setColor(0, 0, 0);
80+
}
81+
82+
public void setColorInt(int color) {
83+
setColor((color >> 16) & 0xFF, (color >> 8) & 0xFF, color & 0xFF);
84+
}
85+
86+
// -------------------------------------------------------------------------
87+
// Required overrides
88+
// -------------------------------------------------------------------------
89+
90+
@Override
91+
public Manufacturer getManufacturer() {
92+
return Manufacturer.Other;
93+
}
94+
95+
@Override
96+
public String getDeviceName() {
97+
return "goBILDA Prism LED Driver";
98+
}
99+
100+
// -------------------------------------------------------------------------
101+
// Private helpers
102+
// -------------------------------------------------------------------------
103+
104+
private void write8(int register, int value) {
105+
deviceClient.write8(register, value, I2cWaitControl.NONE);
106+
}
107+
108+
private void writeChannel(int channelBase, int pwmValue) {
109+
byte[] data = new byte[4];
110+
data[0] = 0x00;
111+
data[1] = 0x00;
112+
data[2] = (byte) (pwmValue & 0xFF);
113+
data[3] = (byte) ((pwmValue >> 8) & 0x0F);
114+
deviceClient.write(channelBase, data, I2cWaitControl.NONE);
115+
}
116+
117+
private int scale(int value8) {
118+
return (value8 * MAX_PWM) / 255;
119+
}
120+
121+
private int clamp(int value, int min, int max) {
122+
return Math.max(min, Math.min(max, value));
123+
}
124+
125+
}

‎TeamCode/src/main/java/org/mrpsvt/capital_robotics/auto/blue_close.java‎

Lines changed: 65 additions & 55 deletions
Original file line numberDiff line numberDiff line change
@@ -21,12 +21,16 @@ public class blue_close extends LinearOpMode {
2121

2222
// Servos
2323
private Servo claw;
24+
private Servo loop;
2425

2526
// Constants
2627
private static final int TARGET_FLYWHEEL = 6000;
2728
private static final int RAMP_RATE = 900;
2829
private static final double CLAW_CLOSED = 1.0;
2930
private static final double CLAW_OPEN = 0.25;
31+
private static final double loop_open = 0;
32+
private static final double loop_closed= 5;
33+
3034
private static final double LODEWHEEL_SPEED = 8;
3135

3236
@Override
@@ -91,49 +95,67 @@ private void autonomousSequence() throws InterruptedException {
9195
// Step 2: Drive forward to position
9296
telemetry.addData("Step", "2: Driving forward");
9397
telemetry.update();
94-
driveDistance(-1, 0, 0, 1500);
95-
//-forwers +back weras
96-
telemetry.addData("Step", "2: Driving forward");
97-
telemetry.update();
98-
driveDistance(0, 0, -1, 250);
99-
//+ to the rigth - to the lefft
100-
// Step 3: Spin up flywheels
101-
telemetry.addData("Step", "3: Spinning up flywheels");
102-
telemetry.update();
103-
rampFlywheels(TARGET_FLYWHEEL, 3000);
104-
98+
driveDistance(.5, 0, 0, 900);
10599

106-
telemetry.addData("Step", "1: Closing claw");
107-
telemetry.update();
108-
setClaw(CLAW_OPEN);
109-
sleep(500);
110-
// Step 4: Run lodewheel to push ball into flywheels and launch
100+
//+ to the rigth - to the lefft
101+
// // Step 3: Spin up flywheels
102+
// telemetry.addData("Step", "3: Spinning up flywheels");
103+
// telemetry.update();
104+
// Flywheelon();
105+
// sleep(3000);
106+
//
107+
// telemetry.addData("Step", "1: Closing claw");
108+
// telemetry.update();
109+
// setClaw(CLAW_OPEN);
110+
// sleep(500);
111+
//
112+
// telemetry.addData("Step", "1: Closing claw");
113+
// telemetry.update();
114+
// setClaw(CLAW_CLOSED);
115+
// sleep(500);
116+
// // Step 4: Run lodewheel to push ball into flywheels and launch
111117
// telemetry.addData("Step", "4: Pushing ball and launching");
112118
// telemetry.update();
113119
// lodewheel.setVelocity(LODEWHEEL_SPEED);
114120
// sleep(2000);
115121
// lodewheel.setVelocity(0);
122+
//
123+
// telemetry.addData("Step", "1: Closing claw");
124+
// telemetry.update();
125+
// setClaw(CLAW_OPEN);
126+
// sleep(500);
127+
//
128+
// telemetry.addData("Step", "1: Closing claw");
129+
// telemetry.update();
130+
// setClaw(CLAW_CLOSED);
131+
// sleep(500);
116132

117-
// Step 5: Spin down flywheels
118-
telemetry.addData("Step", "5: Spinning down");
119-
telemetry.update();
120-
rampFlywheels(0, 2000);
133+
// telemetry.addData("Step", "1: Closing loop");
134+
// telemetry.update();
135+
// setloop(loop_closed);
136+
// sleep(500);
137+
//
138+
// telemetry.addData("Step", "1: Closing loop");
139+
// telemetry.update();
140+
// setloop(loop_open);
141+
// sleep(500);
121142

122-
// Step 6: Strafe right
123-
// telemetry.addData("Step", "6: Strafing right");
143+
// telemetry.addData("Step", "1: Closing claw");
124144
// telemetry.update();
125-
// driveDistance(0, 0.5, 0, 1000);
145+
// setClaw(CLAW_OPEN);
146+
// sleep(500);
126147

127-
// Step 7: Open claw
128-
telemetry.addData("Step", "7: Opening claw");
148+
// // Step 5: Spin down flywheels
149+
// telemetry.addData("Step", "5: Spinning down");
150+
// telemetry.update();
151+
// Flywheeloff();
152+
// sleep(3000);
153+
154+
155+
telemetry.addData("Step", "6: Strafing right");
129156
telemetry.update();
130-
setClaw(CLAW_OPEN);
131-
sleep(500);
157+
driveDistance(0, 1.5, 0, 700);
132158

133-
// Step 8: Drive backward
134-
// telemetry.addData("Step", "8: Driving backward");
135-
// telemetry.update();
136-
// driveDistance(-0.3, 0, 0, 800);
137159

138160
telemetry.addData("Status", "Autonomous Complete");
139161
telemetry.update();
@@ -173,32 +195,20 @@ private void driveDistance(double forward, double strafe, double turn, long mill
173195
backLeft.setPower(0);
174196
backRight.setPower(0);
175197
}
176-
177-
private void rampFlywheels(int targetVelocity, long rampTimeMs) {
178-
long startTime = System.currentTimeMillis();
179-
double currentVelocity = flywheel.getVelocity();
180-
181-
while (System.currentTimeMillis() - startTime < rampTimeMs && opModeIsActive()) {
182-
double progress = (double)(System.currentTimeMillis() - startTime) / rampTimeMs;
183-
double velocity = currentVelocity + (targetVelocity - currentVelocity) * progress;
184-
185-
flywheel.setVelocity(velocity);
186-
flywheel2.setVelocity(velocity);
187-
188-
telemetry.addData("Flywheel Target", targetVelocity);
189-
telemetry.addData("Flywheel Current", velocity);
190-
telemetry.addData("Flywheel1 Actual", flywheel.getVelocity());
191-
telemetry.addData("Flywheel2 Actual", flywheel2.getVelocity());
192-
telemetry.update();
193-
194-
sleep(50);
195-
}
196-
197-
flywheel.setVelocity(targetVelocity);
198-
flywheel2.setVelocity(targetVelocity);
198+
private void Flywheelon(){
199+
flywheel.setVelocity(TARGET_FLYWHEEL);
200+
flywheel2.setVelocity(TARGET_FLYWHEEL);
201+
telemetry.addData("flywheel","on at"+TARGET_FLYWHEEL );
202+
telemetry.update();
203+
}
204+
private void Flywheeloff (){
205+
flywheel.setVelocity(0);
206+
flywheel2.setVelocity(0);
207+
telemetry.addData("flywheel","off");
208+
telemetry.update();
199209
}
200-
201210
private void setClaw(double position) {
202211
claw.setPosition(position);
203212
}
213+
private void setloop (double position){loop.setPosition(position);}
204214
}

‎TeamCode/src/main/java/org/mrpsvt/capital_robotics/auto/blue_far.java‎

Lines changed: 9 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,15 @@ private void autonomousSequence() throws InterruptedException {
8888
// Step 1: Close claw to grab preloaded game element
8989

9090

91+
92+
telemetry.addData("step", "0: temporary auto strafe");
93+
telemetry.update();
94+
driveDistance(0, 1.5, 0, 500);
95+
96+
97+
9198
// Step 2: Drive forward to position
99+
/*
92100
telemetry.addData("Step", "2: Driving forward");
93101
telemetry.update();
94102
driveDistance(-1, 0, 0, 1500);
@@ -102,7 +110,7 @@ private void autonomousSequence() throws InterruptedException {
102110
telemetry.update();
103111
rampFlywheels(TARGET_FLYWHEEL, 3000);
104112
105-
113+
*/
106114
telemetry.addData("Step", "1: Closing claw");
107115
telemetry.update();
108116
setClaw(CLAW_OPEN);

0 commit comments

Comments
 (0)