Skip to content

Commit c783542

Browse files
arfrie22arfrie22
authored
Merge pull request #22 from team467/custom-controller
Switched controller to new custom controller base and added led support
2 parents 6b80af5 + 12f9999 commit c783542

7 files changed

Lines changed: 391 additions & 35 deletions

File tree

‎src/main/java/frc/robot/Robot.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@
77
import edu.wpi.first.wpilibj.TimedRobot;
88
import edu.wpi.first.wpilibj2.command.Command;
99
import edu.wpi.first.wpilibj2.command.CommandScheduler;
10+
import frc.robot.controllers.ControllerQueue;
1011
import frc.robot.tuning.TunerManager;
1112

1213
import java.io.IOException;
@@ -66,6 +67,7 @@ public void robotPeriodic() {
6667
// and running subsystem periodic() methods. This must be called from the robot's periodic
6768
// block in order for anything in the Command-based framework to work.
6869
CommandScheduler.getInstance().run();
70+
ControllerQueue.getInstance().run();
6971
}
7072

7173
/** This function is called once each time the robot enters Disabled mode. */

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 24 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -58,9 +58,12 @@
5858
import frc.robot.commands.ShooterStopFlywheelCMD;
5959
import frc.robot.commands.ShooterTriggerForwardCMD;
6060
import frc.robot.commands.ShooterTriggerStopCMD;
61+
import frc.robot.controllers.ControllerQueue;
6162
import frc.robot.commands.Spitter2022StopCMD;
6263
import frc.robot.commands.TurnToTargetCMD;
6364
import frc.robot.controllers.CustomController2020;
65+
import frc.robot.controllers.CustomController2022;
66+
import frc.robot.controllers.CustomControllerBase;
6467
import frc.robot.controllers.XboxController467;
6568
import frc.robot.led.LEDManager;
6669
import frc.robot.subsystems.Climber2020;
@@ -135,31 +138,16 @@ public class RobotContainer {
135138
XboxController467.Buttons.BumperRight.value);
136139

137140
// Custom controller for operator
138-
private final GenericHID operatorJoystick = new Joystick(1);
139-
private final JoystickButton operatorInakeArm = new JoystickButton(operatorJoystick,
140-
CustomController2020.Buttons.INTAKE_ARM.value);
141-
private final JoystickButton operatorIntakeRollerForward = new JoystickButton(operatorJoystick,
142-
CustomController2020.Buttons.INTAKE_ROLLER_FORWARD.value);
143-
private final JoystickButton operatorIntakeRollerBackward = new JoystickButton(operatorJoystick,
144-
CustomController2020.Buttons.INTAKE_ROLLER_BACKWARD.value);
145-
private final JoystickButton operatorIndexAuto = new JoystickButton(operatorJoystick,
146-
CustomController2020.Buttons.INDEX_AUTO.value);
147-
private final JoystickButton operatorIndexRollerForward = new JoystickButton(operatorJoystick,
148-
CustomController2020.Buttons.INDEX_ROLLER_FORWARD.value);
149-
private final JoystickButton operatorIndexRollerBackward = new JoystickButton(operatorJoystick,
150-
CustomController2020.Buttons.INDEX_ROLLER_BACKWARD.value);
151-
private final JoystickButton operatorShooterAuto = new JoystickButton(operatorJoystick,
152-
CustomController2020.Buttons.SHOOTER_AUTO.value);
153-
private final JoystickButton operatorShooterFlywheel = new JoystickButton(operatorJoystick,
154-
CustomController2020.Buttons.SHOOTER_FLYWHEEL.value);
155-
private final JoystickButton operatorShooterShoot = new JoystickButton(operatorJoystick,
156-
CustomController2020.Buttons.SHOOTER_SHOOT.value);
157-
private final JoystickButton operatorClimberLock = new JoystickButton(operatorJoystick,
158-
CustomController2020.Buttons.CLIMBER_LOCK_SWITCH.value);
159-
private final JoystickButton operatorClimberUp = new JoystickButton(operatorJoystick,
160-
CustomController2020.Buttons.CLIMBER_UP_BUTTON.value);
161-
private final JoystickButton operatorClimberDown = new JoystickButton(operatorJoystick,
162-
CustomController2020.Buttons.CLIMBER_DOWN_BUTTON.value);
141+
private final CustomController2022 operatorJoystick = new CustomController2022(1);
142+
private final JoystickButton operatorFlush = operatorJoystick.getButton(CustomController2022.Buttons.FLUSH);
143+
private final JoystickButton operatorClimberLimits = operatorJoystick.getButton(CustomController2022.Buttons.CLIMBER_LIMITS);
144+
private final JoystickButton operatorShooterAuto = operatorJoystick.getButton(CustomController2022.Buttons.SHOOTER_AUTO);
145+
private final JoystickButton operatorEverything = operatorJoystick.getButton(CustomController2022.Buttons.EVERYTHING);
146+
private final JoystickButton operatorShoot = operatorJoystick.getButton(CustomController2022.Buttons.SHOOT);
147+
private final JoystickButton operatorClimberLock = operatorJoystick.getButton(CustomController2022.Buttons.CLIMBER_LOCK);
148+
private final JoystickButton operatorClimberUp = operatorJoystick.getButton(CustomController2022.Buttons.CLIMBER_UP);
149+
private final JoystickButton operatorClimberDown = operatorJoystick.getButton(CustomController2022.Buttons.CLIMBER_DOWN);
150+
163151

164152
public RobotContainer() {
165153
getTrajectories();
@@ -174,6 +162,7 @@ public RobotContainer() {
174162

175163
// Configure the button bindings
176164
configureButtonBindings();
165+
ControllerQueue.getInstance().addController(operatorJoystick);
177166
}
178167

179168
public void getTrajectories() {
@@ -313,11 +302,11 @@ private void initShooter2020() {
313302

314303
private void configureShooter2020() {
315304
if (RobotConstants.get().hasShooter2020()) {
316-
operatorShooterFlywheel.whenPressed(new ShooterRunFlywheelCMD(shooter));
317-
operatorShooterFlywheel.whenReleased(new ShooterStopFlywheelCMD(shooter));
318-
operatorShooterShoot.whenPressed(new ShooterTriggerForwardCMD(shooter));
319-
operatorShooterShoot.whenReleased(new ShooterTriggerStopCMD(shooter));
320-
operatorShooterShoot.whenReleased(new ShooterTriggerStopCMD(shooter));
305+
operatorEverything.whenPressed(new ShooterRunFlywheelCMD(shooter));
306+
operatorEverything.whenReleased(new ShooterStopFlywheelCMD(shooter));
307+
operatorShoot.whenPressed(new ShooterTriggerForwardCMD(shooter));
308+
operatorShoot.whenReleased(new ShooterTriggerStopCMD(shooter));
309+
operatorShoot.whenReleased(new ShooterTriggerStopCMD(shooter));
321310
}
322311
}
323312

@@ -370,7 +359,7 @@ private void configureClimber2022() {
370359
operatorClimberLock.whenPressed(new Climber2022EnableCMD(climber2022));
371360
operatorClimberLock.whenReleased(new Climber2022DisableCMD(climber2022));
372361
operatorClimberUp.whileHeld(new Climber2022UpCMD(climber2022));
373-
operatorClimberDown.whileHeld(new Climber2022DownCMD(climber2022, operatorIndexAuto::get));
362+
operatorClimberDown.whileHeld(new Climber2022DownCMD(climber2022, operatorClimberLimits::get));
374363
}
375364
}
376365

@@ -398,24 +387,24 @@ private void configureShooter2022() {
398387
if (RobotConstants.get().hasLlamaNeck2022()
399388
&& RobotConstants.get().hasIndexer2022()
400389
&& RobotConstants.get().hasSpitter2022()) {
401-
if (operatorShooterFlywheel.get()) {
390+
if (operatorEverything.get()) {
402391
shooter2022.setDefaultCommand(
403392
new Shooter2022IdleSpinupCMD(shooter2022, () -> Spitter2022.getFlywheelVelocity(0.6)));
404393
} else {
405394
shooter2022.setDefaultCommand(
406395
new Shooter2022StopCMD(shooter2022));
407396
}
408397

409-
operatorShooterFlywheel
398+
operatorEverything
410399
.whenPressed(
411400
new Shooter2022SetDefaultCMD(
412401
shooter2022, new Shooter2022IdleSpinupCMD(shooter2022, () -> Spitter2022.getFlywheelVelocity(0.6))))
413402
.whenReleased(
414403
new Shooter2022SetDefaultCMD(
415404
shooter2022, new Shooter2022StopCMD(shooter2022)));
416-
operatorShooterShoot.whenPressed(
405+
operatorShoot.whenPressed(
417406
new Shooter2022ShootTargetCMD(shooter2022));
418-
operatorIntakeRollerBackward.whenHeld(
407+
operatorFlush.whenHeld(
419408
new Shooter2022FlushBallCMD(shooter2022));
420409
}
421410
}

‎src/main/java/frc/robot/controllers/ControllerCommandFactory.java‎

Lines changed: 170 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,170 @@
1+
package frc.robot.controllers;
2+
3+
import edu.wpi.first.wpilibj.util.Color;
4+
import edu.wpi.first.wpilibj.util.Color8Bit;
5+
6+
public class ControllerCommandFactory {
7+
public static byte[] setLEDRGB(int index, int r, int g, int b) {
8+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Single, (byte) index, ControllerDataProtocol.LightingValue.LedBaseColor, (byte) r, (byte) g, (byte) b};
9+
}
10+
11+
public static byte[] setLEDBaseColor(int index, Color8Bit color) {
12+
return setLEDRGB(index, color.red, color.green, color.blue);
13+
}
14+
15+
public static byte[] setLEDBaseColor(int index, Color color) {
16+
return setLEDBaseColor(index, new Color8Bit(color));
17+
}
18+
19+
public static byte[] setLEDHSB(int index, float h, float s, float b) {
20+
java.awt.Color outColor = java.awt.Color.getHSBColor(h, s, b);
21+
return setLEDRGB(index, outColor.getRed(), outColor.getGreen(), outColor.getBlue());
22+
}
23+
24+
public static byte[] setLEDHSB(int index, int h, int s, int b) {
25+
return setLEDHSB(index, h/360f, s/255f, b/255f);
26+
}
27+
28+
public static byte[] setLEDRangeRGB(int startLED, int endLED, int r, int g, int b) {
29+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Multiple, (byte) startLED, (byte) endLED, ControllerDataProtocol.LightingValue.LedBaseColor, (byte) r, (byte) g, (byte) b};
30+
}
31+
32+
public static byte[] setLEDRangeBaseColor(int startLED, int endLED, Color8Bit color) {
33+
return setLEDRangeRGB(startLED, endLED, color.red, color.green, color.blue);
34+
}
35+
36+
public static byte[] setLEDRangeBaseColor(int startLED, int endLED, Color color) {
37+
return setLEDRangeBaseColor(startLED, endLED, new Color8Bit(color));
38+
}
39+
40+
public static byte[] setLEDRangeHSB(int startLED, int endLED, float h, float s, float b) {
41+
java.awt.Color outColor = java.awt.Color.getHSBColor(h, s, b);
42+
return setLEDRangeRGB(startLED, endLED, outColor.getRed(), outColor.getGreen(), outColor.getBlue());
43+
}
44+
45+
public static byte[] setLEDRangeHSB(int startLED, int endLED, int h, int s, int b) {
46+
return setLEDRangeHSB(startLED, endLED, h/360f, s/255f, b/255f);
47+
}
48+
49+
public static byte[] setLEDSectionRGB(int section, int r, int g, int b) {
50+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Section, (byte) section, ControllerDataProtocol.LightingValue.LedBaseColor, (byte) r, (byte) g, (byte) b};
51+
}
52+
53+
public static byte[] setLEDSectionBaseColor(int section, Color8Bit color) {
54+
return setLEDSectionRGB(section, color.red, color.green, color.blue);
55+
}
56+
57+
public static byte[] setLEDSectionBaseColor(int section, Color color) {
58+
return setLEDSectionBaseColor(section, new Color8Bit(color));
59+
}
60+
61+
public static byte[] setLEDSectionHSB(int section, float h, float s, float b) {
62+
java.awt.Color outColor = java.awt.Color.getHSBColor(h, s, b);
63+
return setLEDSectionRGB(section, outColor.getRed(), outColor.getGreen(), outColor.getBlue());
64+
}
65+
66+
public static byte[] setLEDSectionHSB(int section, int h, int s, int b) {
67+
return setLEDSectionHSB(section, h/360f, s/255f, b/255f);
68+
}
69+
70+
public static byte[] setAllLEDRGB(int r, int g, int b) {
71+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.All, ControllerDataProtocol.LightingValue.LedBaseColor, (byte) r, (byte) g, (byte) b};
72+
}
73+
74+
public static byte[] setAllLEDBaseColor(Color8Bit color) {
75+
return setAllLEDRGB(color.red, color.green, color.blue);
76+
}
77+
78+
public static byte[] setAllLEDBaseColor(Color color) {
79+
return setAllLEDBaseColor(new Color8Bit(color));
80+
}
81+
82+
public static byte[] setAllLEDHSB(float h, float s, float b) {
83+
java.awt.Color outColor = java.awt.Color.getHSBColor(h, s, b);
84+
return setAllLEDRGB(outColor.getRed(), outColor.getGreen(), outColor.getBlue());
85+
}
86+
87+
public static byte[] setAllLEDHSB(int h, int s, int b) {
88+
return setAllLEDHSB(h/360f, s/255f, b/255f);
89+
}
90+
91+
92+
public static byte[] setLEDEffect(int index, int effect) {
93+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Single, (byte) index, ControllerDataProtocol.LightingValue.LedEffect, (byte) effect};
94+
}
95+
96+
public static byte[] setLEDRangeEffect(int startLED, int endLED, int effect) {
97+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Multiple, (byte) startLED, (byte) endLED, ControllerDataProtocol.LightingValue.LedEffect, (byte) effect};
98+
}
99+
100+
public static byte[] setLEDSectionEffect(int section, int effect) {
101+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Section, (byte) section, ControllerDataProtocol.LightingValue.LedEffect, (byte) effect};
102+
}
103+
104+
public static byte[] setAllLEDEffect(int effect) {
105+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.All, ControllerDataProtocol.LightingValue.LedEffect, (byte) effect};
106+
}
107+
108+
public static byte[] setLEDRangeEffectSpaced(int startLED, int endLED, int effect) {
109+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Multiple, (byte) startLED, (byte) endLED, ControllerDataProtocol.LightingValue.LedEffectSpaced, (byte) effect};
110+
}
111+
112+
public static byte[] setLEDSectionEffectSpaced(int section, int effect) {
113+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Section, (byte) section, ControllerDataProtocol.LightingValue.LedEffectSpaced, (byte) effect};
114+
}
115+
116+
public static byte[] setAllLEDEffectSpaced(int effect) {
117+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.All, ControllerDataProtocol.LightingValue.LedEffectSpaced, (byte) effect};
118+
}
119+
120+
121+
public static byte[] setLEDOffset(int index, int offset) {
122+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Single, (byte) index, ControllerDataProtocol.LightingValue.LedOffset, (byte) offset};
123+
}
124+
125+
public static byte[] setLEDRangeOffset(int startLED, int endLED, int offset) {
126+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Multiple, (byte) startLED, (byte) endLED, ControllerDataProtocol.LightingValue.LedOffset, (byte) offset};
127+
}
128+
129+
public static byte[] setLEDSectionOffset(int section, int offset) {
130+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Section, (byte) section, ControllerDataProtocol.LightingValue.LedOffset, (byte) offset};
131+
}
132+
133+
public static byte[] setAllLEDOffset(int offset) {
134+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.All, ControllerDataProtocol.LightingValue.LedOffset, (byte) offset};
135+
}
136+
137+
138+
public static byte[] setLEDSpeed(int index, int speed) {
139+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Single, (byte) index, ControllerDataProtocol.LightingValue.LedSpeed, (byte) speed};
140+
}
141+
142+
public static byte[] setLEDRangeSpeed(int startLED, int endLED, int speed) {
143+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Multiple, (byte) startLED, (byte) endLED, ControllerDataProtocol.LightingValue.LedSpeed, (byte) speed};
144+
}
145+
146+
public static byte[] setLEDSectionSpeed(int section, int speed) {
147+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Section, (byte) section, ControllerDataProtocol.LightingValue.LedSpeed, (byte) speed};
148+
}
149+
150+
public static byte[] setAllLEDSpeed(int speed) {
151+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.All, ControllerDataProtocol.LightingValue.LedSpeed, (byte) speed};
152+
}
153+
154+
155+
public static byte[] setLEDBrightness(int index, int brightness) {
156+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Single, (byte) index, ControllerDataProtocol.LightingValue.LedBrightness, (byte) brightness};
157+
}
158+
159+
public static byte[] setLEDRangeBrightness(int startLED, int endLED, int brightness) {
160+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Multiple, (byte) startLED, (byte) endLED, ControllerDataProtocol.LightingValue.LedBrightness, (byte) brightness};
161+
}
162+
163+
public static byte[] setLEDSectionBrightness(int section, int brightness) {
164+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.Section, (byte) section, ControllerDataProtocol.LightingValue.LedBrightness, (byte) brightness};
165+
}
166+
167+
public static byte[] setAllLEDBrightness(int brightness) {
168+
return new byte[] {ControllerDataProtocol.CommandID.SetLed, ControllerDataProtocol.LightingSelection.All, ControllerDataProtocol.LightingValue.LedBrightness, (byte) brightness};
169+
}
170+
}

‎src/main/java/frc/robot/controllers/ControllerDataProtocol.java‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
package frc.robot.controllers;
2+
3+
public class ControllerDataProtocol {
4+
public static class CommandID {
5+
public static byte GetProtocolVersion = 0x01;
6+
public static byte GetTeamNumber = 0x02;
7+
public static byte GetControllerState = 0x03;
8+
public static byte GetLedData = 0x04;
9+
public static byte GetLed = 0x05;
10+
public static byte SetLed = 0x06;
11+
12+
13+
//...
14+
public static byte GetPortName = (byte) 0xFD;
15+
public static byte EnterBootloader = (byte) 0xFE;
16+
public static byte Error = (byte) 0xFF;
17+
}
18+
19+
public static class LedData {
20+
public static byte LedCount = 0x01;
21+
public static byte SectionCount = 0x02;
22+
}
23+
24+
public static class LightingSelection {
25+
public static byte Single = 0x01;
26+
public static byte Multiple = 0x02;
27+
public static byte Section = 0x03;
28+
public static byte All = 0x04;
29+
}
30+
31+
public static class LightingValue {
32+
public static byte LedBaseColor = 0x01;
33+
public static byte LedEffect = 0x02;
34+
public static byte LedEffectSpaced = 0x03;
35+
public static byte LedOffset = 0x04;
36+
public static byte LedSpeed = 0x05;
37+
public static byte LedBrightness = 0x06;
38+
}
39+
40+
public static class LightingEffect {
41+
public static byte Static = 0x00;
42+
public static byte BreathingUp = 0x01;
43+
public static byte BreathingDown = 0x02;
44+
public static byte ColorCycle = 0x03;
45+
}
46+
}

‎src/main/java/frc/robot/controllers/ControllerQueue.java‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
package frc.robot.controllers;
2+
3+
import java.util.ArrayList;
4+
5+
public class ControllerQueue {
6+
private static ControllerQueue instance = null;
7+
8+
private ArrayList<CustomControllerBase> controllers = new ArrayList<>();
9+
10+
public static ControllerQueue getInstance() {
11+
if (instance == null) instance = new ControllerQueue();
12+
13+
return instance;
14+
}
15+
16+
public void addController(CustomControllerBase controller) {
17+
controllers.add(controller);
18+
}
19+
20+
public void run() {
21+
controllers.forEach(controller -> {
22+
controller.updateQueue();
23+
});
24+
}
25+
26+
public CustomControllerBase get(int index) {
27+
return controllers.get(index);
28+
}
29+
}

0 commit comments

Comments
 (0)