Skip to content

Commit d535a52

Browse files
DriverStationComputerDriverStationComputer
committed
Merge branch 'bangbot' of https://github.com/deepbluerobotics/RobotCode2024 into bangbot
2 parents 46b33ed + 7f3fa75 commit d535a52

File tree

4 files changed

+197
-47
lines changed

4 files changed

+197
-47
lines changed

‎simgui.json‎

Lines changed: 16 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,13 @@
11
{
22
"HALProvider": {
3+
"Addressable LEDs": {
4+
"0": {
5+
"columns": 32
6+
},
7+
"window": {
8+
"visible": true
9+
}
10+
},
311
"Other Devices": {
412
"CANCoder (v6)[0]": {
513
"header": {
@@ -55,6 +63,13 @@
5563
"/SmartDashboard/SendableChooser[0]": "String Chooser"
5664
}
5765
},
66+
"NetworkTables": {
67+
"transitory": {
68+
"SmartDashboard": {
69+
"open": true
70+
}
71+
}
72+
},
5873
"NetworkTables Info": {
5974
"Clients": {
6075
"open": true
@@ -71,7 +86,7 @@
7186
0.0,
7287
0.8500000238418579
7388
],
74-
"height": 621
89+
"height": 177
7590
}
7691
]
7792
}

‎src/main/java/org/carlmontrobotics/Constants.java‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,10 @@ public static final class Led {
5151
public static final int ledLength = 1000;//lower doesn't work but higher does
5252
public static final int ledPort = 5;
5353

54-
public static final Color8Bit defaultColor = new Color8Bit(0, 0, 200);
54+
//public static final Color8Bit defaultColor = new Color8Bit(0, 0, 200);
55+
56+
57+
5558
public static final Color8Bit detectNote = new Color8Bit(250,140,3);
5659
public static final Color8Bit holding = new Color8Bit(0,250,0);
5760
public static final Color8Bit ejectColor = new Color8Bit(250,250,0);
@@ -61,6 +64,10 @@ public static final class Led {
6164
public static final Color8Bit intakeouttakeColor = new Color8Bit(0,255,9); //intake and outtake (both) = green
6265
//Red when nothing, purple/blue when intake/outtake detect only, green when both
6366

67+
68+
//for weird alex stuf
69+
public static Color8Bit startingColor = new Color8Bit(255,0,0);
70+
6471
}
6572

6673
public static final class Effectorc {

‎src/main/java/org/carlmontrobotics/RobotContainer.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -83,6 +83,7 @@ public class RobotContainer {
8383
private final IntakeShooter intakeShooter = new IntakeShooter();
8484
private final Arm arm = new Arm();
8585
private final Drivetrain drivetrain = new Drivetrain();
86+
private final AuxSystems auxSystems = new AuxSystems(arm, intakeShooter);
8687

8788
/* These must be equal to the pathPlanner path names from the GUI! */
8889
// Order matters - but the first one is index 1 on the physical selector - index 0 is reserved for null command.

‎src/main/java/org/carlmontrobotics/subsystems/AuxSystems.java‎

Lines changed: 172 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -2,83 +2,210 @@
22
import edu.wpi.first.wpilibj2.command.SubsystemBase;
33
import org.carlmontrobotics.subsystems.IntakeShooter;
44

5+
import com.ctre.phoenix.led.ColorFlowAnimation;
56

67
import edu.wpi.first.wpilibj.AddressableLED;
78
import edu.wpi.first.wpilibj.AddressableLEDBuffer;
9+
import edu.wpi.first.wpilibj.util.Color;
810
import edu.wpi.first.wpilibj.util.Color8Bit;
11+
12+
import static org.carlmontrobotics.Constants.Effectorc.*;
13+
14+
import static org.carlmontrobotics.Constants.Led.startingColor;
15+
916
import org.carlmontrobotics.Constants;
1017
import org.carlmontrobotics.commands.Intake;
18+
import edu.wpi.first.wpilibj.Timer;
19+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
1120

1221

13-
/**
14-
* AuxSystems contains both LED and Rumble functionality,
22+
/**
23+
* AuxSystems contains both LED and Rumble functionality,
1524
* and is separate because of it's special needs to take in other subsystems.
1625
*/
17-
public class AuxSystems {
26+
public class AuxSystems extends SubsystemBase {
1827
private final AddressableLEDBuffer ledBuffer = new AddressableLEDBuffer(Constants.Led.ledLength);
1928
private final AddressableLED led = new AddressableLED(Constants.Led.ledPort);
2029
private int midpoint = (int) Math.floor(ledBuffer.getLength()/2);//rounds down
30+
//AuxSubsystems watches the two other subsystems in order to rumble/led in peace
31+
//public final Color8Bit defaultColor = new Color8Bit(255,0,0);
32+
private final Arm arm;
33+
private final IntakeShooter effector;
34+
//for led color ramping
35+
private Color8Bit currentColor = new Color8Bit(0,0,0);
2136

22-
private final IntakeShooter endEffector;
23-
2437

25-
public AuxSystems(IntakeShooter is){
26-
endEffector = is;
38+
private LedPoint currentPoint = new LedPoint();
39+
public boolean matchMode = false;
40+
//^ whether we use normal color checking or flowing color mode
41+
42+
private double rumble;
43+
//stuf
44+
45+
public AuxSystems(Arm a, IntakeShooter is){
46+
this.arm = a;
47+
this.effector = is;
2748
//leds
2849
led.setLength(ledBuffer.getLength());
29-
setLedColor(Constants.Led.defaultColor, 0 , ledBuffer.getLength());
50+
setLedColor(startingColor, 0 , ledBuffer.getLength());
3051
led.start();
52+
53+
54+
SmartDashboard.putBoolean("LED Match Mode:", matchMode);
55+
56+
// int r = 0;
57+
// int g = 0;
58+
// int b = 0;
59+
// //weird alex stuff
60+
// for (int red = 0; red < 255/15; red ++){
61+
// for (int blue = 0; blue < 255/15; blue ++){
62+
// for (int green = 0; green < 255/15; green ++){
63+
// g +=15;
64+
65+
// currentPoint = currentPoint.addnext(0.1,new Color8Bit(r,g,b));
66+
67+
// }
68+
// g = 0;
69+
// b +=15;
70+
71+
72+
// }
73+
// b = 0;
74+
// r +=15;
75+
76+
77+
//}
78+
Color8Bit[] colorCycle = {
79+
new Color8Bit(255,0,0),
80+
new Color8Bit(255,127,0),
81+
new Color8Bit(255,255,0),
82+
new Color8Bit(127,255,0),
83+
new Color8Bit(0,255,0),
84+
new Color8Bit(0,255,127),
85+
new Color8Bit(0,255,255),
86+
new Color8Bit(0,127,255),
87+
new Color8Bit(0,127,255),
88+
new Color8Bit(0,0,255),
89+
new Color8Bit(127,0,255),
90+
new Color8Bit(255,0,255),
91+
new Color8Bit(255,0,127),
92+
93+
};
94+
for (Color8Bit color : colorCycle){
95+
currentPoint = currentPoint.addnext(2,color);
96+
97+
}
98+
99+
100+
101+
currentPoint.start();
31102
}
32103

33104
public void setLedColor(Color8Bit color, int start, int end) {
34105
for (int i = start; i < end ; i++)
35106
ledBuffer.setRGB(i, color.red, color.green, color.blue);
36107
led.setData(ledBuffer);
37108
}
38-
}
39-
40-
// nothing : setLedColor(defaultColr, 0, ledBuffer.getLength())
41-
//first tof detect: setLedColor(detectNote, 0, midpoint)
42-
//second tof detec: setLedColor(detectNote, midpoint, ledBuffer.getLength())
43-
//both tof detect: setLedColor(holding, 0, ledBuffer.getLength())
44-
45-
/*in arm code make it so that color changes to intakeColor when distance sensors detect a note
46-
(using either boolean noteInIntake or boolean intakeDetectsNote)
47-
there is definitely a shorter way to write this
48-
49-
public void changeIntakeColor(){
50-
setLedColor(intakeColor);
51-
resetColorCommand.schedule();
109+
@Override
110+
public void periodic(){
111+
matchMode = SmartDashboard.getBoolean("LED Match Mode:", matchMode);
112+
if (matchMode){
113+
/** LED RULES
114+
* only intake TOF: lower yellow
115+
* only outake TOF: upper purple
116+
* both TOF: all green
117+
* none: all red
118+
*/
119+
/** RUMBLE RULES
120+
* at setpoint + intake TOF: rumble
121+
* only outake TOF: rumble
122+
* else: no rumble
123+
* none: all red
124+
*/
125+
126+
127+
}else {
128+
currentColor = currentPoint.currentDesiredColor();
129+
setLedColor(currentColor,0,Constants.Led.ledLength);
130+
131+
52132
}
133+
134+
53135

54-
if(noteInIntake){
55-
changeIntakeColor();
56-
}
136+
}
57137

58-
public void changeOuttakeColor(){
59-
setLedColor(outtakeColor);
60-
resetColorCommand.schedule();
61-
}
138+
/**
139+
* LedPoint
140+
*
141+
* Used to represent points of a graph of color, so that you can smoothly transition between them.
142+
* Create an initial point by calling
143+
* LedPoint ledpoint = new LedPoint();
144+
* or so, and then add points to the graph with
145+
* ledpoint = ledpoint.addnext(2, new Color8Bit(255,0,0));
146+
* which would smoothly transition from black (000000) to full red (ff0000) in two seconds
147+
* as long as you called
148+
* setLedColor(ledpoint.currentDesiredColor() ,0,Constants.Led.ledLength);
149+
* every periodic
150+
*/
151+
public class LedPoint {
152+
public double startTime,duration;
153+
public int r,g,b;
154+
public LedPoint parent,child;
62155

63-
if(/boolean value for outtake detection/){
64-
changeOuttakeColor();
156+
//initial black null constructor
157+
public LedPoint() {
158+
this.parent=null;
159+
this.startTime = Timer.getFPGATimestamp();
160+
this.duration = 0;
161+
this.setColor(new Color8Bit(0,0,0));
65162
}
66-
67-
public void changeIntakeOuttakeColor(){
68-
setLedColor(intakeouttakeColor);
69-
resetColorCommand.schedule();
163+
//use parent to chain points together
164+
public LedPoint(LedPoint parent, double timeToTarget, Color8Bit color) {
165+
this.parent=parent;
166+
this.duration = timeToTarget;
167+
this.setColor(color);
70168
}
169+
public double start(){
170+
double now = Timer.getFPGATimestamp();
71171

72-
if(noteInIntake && /boolean value for outtake detection/){
73-
changeIntakeOuttakeColor();
172+
if (parent!=null){
173+
startTime = parent.start() + parent.duration;
174+
} else {
175+
startTime = now;
176+
}
177+
return startTime;
178+
}
179+
//default assumes startTime is NOW
180+
public LedPoint addnext(double timeToTarget, Color8Bit color) {
181+
this.child = new LedPoint(this, timeToTarget, color);
182+
return this.child;
74183
}
75184

76-
private Command resetColorCommand = new SequentialCommandGroup(
77-
new WaitCommand(ledDefaultColorRestoreTime),
78-
new InstantCommand(() -> setLedColor(defaultColor))) {
79-
public boolean runsWhenDisabled() {
80-
return true;
81-
};
82-
}; */
83-
185+
private void setColor(Color8Bit color){
186+
this.r=color.red;
187+
this.g=color.green;
188+
this.b=color.blue;
189+
}
84190

191+
public Color8Bit currentDesiredColor(){//return the color it should be now
192+
double currTime = Timer.getFPGATimestamp();
193+
if (currTime<startTime){//is it our parents turn still?
194+
if (parent==null){//no parent, do nothing
195+
return currentColor;
196+
} else{
197+
return parent.currentDesiredColor();
198+
}
199+
}
200+
double endTime = this.startTime+this.duration;
201+
202+
double dfrac = (endTime-currTime)/(endTime-startTime);
203+
//^ fraction of completion
204+
return new Color8Bit(
205+
currentColor.red + (int) (dfrac*(r-currentColor.red)),
206+
currentColor.green + (int) (dfrac*(g-currentColor.green)),
207+
currentColor.blue + (int) (dfrac*(b-currentColor.blue))
208+
);
209+
}
210+
}
211+
}

0 commit comments

Comments
 (0)