Skip to content

Commit 319bc73

Browse files
mwitcpalekmwitcpalek
authored
Merge pull request #23 from strykeforce/robot-state
Initial Robot State Testing
2 parents 10a82ce + 5565949 commit 319bc73

22 files changed

+498
-201
lines changed

‎README.md‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -127,8 +127,8 @@
127127
## Cameras
128128
| Camera | IP Address | Type |
129129
| ----------- | ----------- | ------- |
130-
| Left Servo | 10.27.67.XX | USB 3.0 |
131-
| Right Servo | 10.27.67.YY | USB 3.0 |
132-
| Upper Left | 10.27.67.XX | USB 2.0 |
133-
| Upper Right | 10.27.67.YY | USB 2.0 |
134-
| Rear | 10.27.67.ZZ | USB 2.0 |
130+
| Left Servo | 10.27.67.11 | USB 3.0 |
131+
| Right Servo | 10.27.67.12 | USB 3.0 |
132+
| Upper Left | 10.27.67.11 | USB 2.0 |
133+
| Upper Right | 10.27.67.12 | USB 2.0 |
134+
| Rear | 10.27.67.13 | USB 2.0 |

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

Lines changed: 105 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -22,12 +22,19 @@
2222
import frc.robot.commands.elevator.JogElevatorCommand;
2323
import frc.robot.commands.elevator.SetElevatorPositionCommand;
2424
import frc.robot.commands.elevator.ZeroElevatorCommand;
25+
import frc.robot.commands.robotState.ScoreReefManualCommand;
26+
import frc.robot.commands.robotState.SetScoringLevelCommand;
27+
import frc.robot.commands.robotState.StowCommand;
2528
import frc.robot.constants.ElevatorConstants;
2629
import frc.robot.constants.RobotConstants;
2730
import frc.robot.controllers.FlyskyJoystick;
2831
import frc.robot.controllers.FlyskyJoystick.Button;
2932
import frc.robot.subsystems.algae.AlgaeIOFX;
3033
import frc.robot.subsystems.algae.AlgaeSubsystem;
34+
import frc.robot.subsystems.battMon.BattMonSubsystem;
35+
import frc.robot.subsystems.biscuit.BiscuitIOFX;
36+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
37+
import frc.robot.subsystems.climb.ClimbSubsystem;
3138
import frc.robot.subsystems.coral.CoralIO;
3239
import frc.robot.subsystems.coral.CoralIOFX;
3340
import frc.robot.subsystems.coral.CoralSubsystem;
@@ -36,53 +43,113 @@
3643
import frc.robot.subsystems.elevator.ElevatorIO;
3744
import frc.robot.subsystems.elevator.ElevatorIOFX;
3845
import frc.robot.subsystems.elevator.ElevatorSubsystem;
46+
import frc.robot.subsystems.funnel.FunnelIOFXS;
47+
import frc.robot.subsystems.funnel.FunnelSubsystem;
48+
import frc.robot.subsystems.led.LEDIO;
49+
import frc.robot.subsystems.led.LEDSubsystem;
50+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
51+
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
52+
import frc.robot.subsystems.tagAlign.TagAlignSubsystem;
53+
import frc.robot.subsystems.vision.VisionSubsystem;
3954
import org.strykeforce.telemetry.TelemetryController;
4055
import org.strykeforce.telemetry.TelemetryService;
4156

4257
public class RobotContainer {
58+
private final RobotStateSubsystem robotStateSubsystem;
4359

44-
private ElevatorIO elevatorIO;
45-
private ElevatorSubsystem elevatorSubsystem;
60+
private final AlgaeIOFX algaeIO;
61+
private final AlgaeSubsystem algaeSubsystem;
62+
63+
private final BattMonSubsystem battMonSubsystem;
64+
65+
private final BiscuitIOFX biscuitIO;
66+
private final BiscuitSubsystem biscuitSubsystem;
67+
68+
private final ClimbSubsystem climbSubsystem;
4669

4770
private final CoralIO coralIO;
4871
private final CoralSubsystem coralSubsystem;
4972

50-
private AlgaeIOFX algaeIO;
51-
private AlgaeSubsystem algaeSubsystem;
73+
private final Swerve swerve;
74+
private final DriveSubsystem driveSubsystem;
75+
76+
private final ElevatorIO elevatorIO;
77+
private final ElevatorSubsystem elevatorSubsystem;
78+
79+
private final FunnelIOFXS funnelIO;
80+
private final FunnelSubsystem funnelSubsystem;
5281

53-
private Swerve swerve;
54-
private DriveSubsystem driveSubsystem;
82+
private final LEDIO ledIO;
83+
private final LEDSubsystem ledSubsystem;
84+
85+
private final TagAlignSubsystem tagAlignSubsystem;
86+
87+
private final VisionSubsystem visionSubsystem;
5588

5689
private final XboxController xboxController = new XboxController(1);
5790
private final Joystick driveJoystick = new Joystick(0);
58-
5991
private final FlyskyJoystick flysky = new FlyskyJoystick(driveJoystick);
92+
6093
private final TelemetryService telemetryService = new TelemetryService(TelemetryController::new);
6194

6295
public RobotContainer() {
96+
6397
algaeIO = new AlgaeIOFX();
6498
algaeSubsystem = new AlgaeSubsystem(algaeIO);
6599

66-
swerve = new Swerve();
67-
driveSubsystem = new DriveSubsystem(swerve);
100+
battMonSubsystem = new BattMonSubsystem();
101+
102+
biscuitIO = new BiscuitIOFX();
103+
biscuitSubsystem = new BiscuitSubsystem(biscuitIO);
104+
105+
climbSubsystem = new ClimbSubsystem();
68106

69107
coralIO = new CoralIOFX();
70108
coralSubsystem = new CoralSubsystem(coralIO);
71109

110+
swerve = new Swerve();
111+
driveSubsystem = new DriveSubsystem(swerve);
112+
72113
elevatorIO = new ElevatorIOFX();
73114
elevatorSubsystem = new ElevatorSubsystem(elevatorIO);
74115

116+
funnelIO = new FunnelIOFXS();
117+
funnelSubsystem = new FunnelSubsystem(funnelIO);
118+
119+
ledIO = new LEDIO();
120+
ledSubsystem = new LEDSubsystem();
121+
122+
visionSubsystem = new VisionSubsystem();
123+
124+
tagAlignSubsystem = new TagAlignSubsystem(driveSubsystem, visionSubsystem);
125+
126+
robotStateSubsystem =
127+
new RobotStateSubsystem(
128+
algaeSubsystem,
129+
battMonSubsystem,
130+
biscuitSubsystem,
131+
climbSubsystem,
132+
coralSubsystem,
133+
driveSubsystem,
134+
elevatorSubsystem,
135+
funnelSubsystem,
136+
ledSubsystem,
137+
tagAlignSubsystem,
138+
visionSubsystem);
139+
140+
driveSubsystem.setRobotStateSubsystem(robotStateSubsystem);
141+
75142
configureTelemetry();
76143
configureDriverBindings();
77144
configureOperatorBindings();
78145
}
79146

80147
private void configureTelemetry() {
81-
telemetryService.register(driveSubsystem);
82-
telemetryService.register(coralSubsystem);
83-
telemetryService.register(algaeSubsystem);
84-
telemetryService.register(elevatorSubsystem);
85-
elevatorIO.registerWith(telemetryService);
148+
driveSubsystem.registerWith(telemetryService);
149+
coralSubsystem.registerWith(telemetryService);
150+
algaeSubsystem.registerWith(telemetryService);
151+
elevatorSubsystem.registerWith(telemetryService);
152+
funnelSubsystem.registerWith(telemetryService);
86153
telemetryService.start();
87154
}
88155

@@ -93,9 +160,33 @@ private void configureDriverBindings() {
93160

94161
// Reset Gyro Command
95162
new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem));
163+
new JoystickButton(driveJoystick, Button.M_SWH.id)
164+
.onTrue(new ScoreReefManualCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem));
96165
}
97166

98167
private void configureOperatorBindings() {
168+
// Set Levels
169+
new Trigger(() -> xboxController.getLeftTriggerAxis() > RobotConstants.kTriggerDeadband)
170+
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L1));
171+
new JoystickButton(xboxController, XboxController.Button.kLeftBumper.value)
172+
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L2));
173+
new JoystickButton(xboxController, XboxController.Button.kRightBumper.value)
174+
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L3));
175+
new Trigger(() -> xboxController.getRightTriggerAxis() > RobotConstants.kTriggerDeadband)
176+
.onTrue(new SetScoringLevelCommand(robotStateSubsystem, ScoringLevel.L4));
177+
178+
// Stow
179+
new JoystickButton(xboxController, XboxController.Button.kBack.value)
180+
.onTrue(
181+
new StowCommand(
182+
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
183+
184+
// zero elevator, slated for removal
185+
new JoystickButton(xboxController, XboxController.Button.kX.value)
186+
.onTrue(new ZeroElevatorCommand(elevatorSubsystem));
187+
}
188+
189+
private void configureTestOperatorBindings() {
99190
// Stop Coral
100191
new JoystickButton(xboxController, XboxController.Button.kB.value)
101192
.onTrue(new OpenLoopCoralCommand(coralSubsystem, 0));

‎src/main/java/frc/robot/commands/robotState/ScoreReefManualCommand.java‎

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.coral.CoralSubsystem;
5+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
6+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
7+
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
8+
9+
public class ScoreReefManualCommand extends Command {
10+
private RobotStateSubsystem robotStateSubsystem;
11+
private ElevatorSubsystem elevatorSubsystem;
12+
private RobotStates startingRobotState;
13+
private boolean startingElevatorFinished;
14+
15+
public ScoreReefManualCommand(
16+
RobotStateSubsystem robotStateSubsystem,
17+
ElevatorSubsystem elevatorSubsystem,
18+
CoralSubsystem coralSubsystem) {
19+
this.robotStateSubsystem = robotStateSubsystem;
20+
this.elevatorSubsystem = elevatorSubsystem;
21+
addRequirements(elevatorSubsystem, coralSubsystem);
22+
}
23+
24+
@Override
25+
public void initialize() {
26+
startingRobotState = robotStateSubsystem.getState();
27+
startingElevatorFinished = elevatorSubsystem.isFinished();
28+
robotStateSubsystem.setIsAuto(false);
29+
robotStateSubsystem.setGetAlgaeOnCycle(false);
30+
robotStateSubsystem.toPrepCoral();
31+
}
32+
33+
@Override
34+
public boolean isFinished() {
35+
if (startingRobotState == RobotStates.PRESTAGE || startingRobotState == RobotStates.STOW) {
36+
return robotStateSubsystem.getState() == RobotStates.REEF_ALIGN_CORAL;
37+
}
38+
if (startingRobotState == RobotStates.REEF_ALIGN_CORAL) {
39+
return robotStateSubsystem.getState() == RobotStates.FUNNEL_LOAD
40+
|| robotStateSubsystem.getState() == RobotStates.LOADING_CORAL
41+
|| !startingElevatorFinished;
42+
}
43+
return false;
44+
}
45+
}

‎src/main/java/frc/robot/commands/robotState/SetScoringLevelCommand.java‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
5+
import frc.robot.subsystems.robotState.RobotStateSubsystem.ScoringLevel;
6+
7+
public class SetScoringLevelCommand extends InstantCommand {
8+
private RobotStateSubsystem robotStateSubsystem;
9+
private ScoringLevel level;
10+
11+
public SetScoringLevelCommand(RobotStateSubsystem robotStateSubsystem, ScoringLevel level) {
12+
this.robotStateSubsystem = robotStateSubsystem;
13+
this.level = level;
14+
}
15+
16+
@Override
17+
public void initialize() {
18+
robotStateSubsystem.setScoringLevel(level);
19+
}
20+
}

‎src/main/java/frc/robot/commands/robotState/StowCommand.java‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
5+
import frc.robot.subsystems.coral.CoralSubsystem;
6+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
7+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
8+
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
9+
10+
public class StowCommand extends Command {
11+
RobotStateSubsystem robotState;
12+
13+
public StowCommand(
14+
RobotStateSubsystem robotState,
15+
ElevatorSubsystem elevatorSubsystem,
16+
CoralSubsystem coralSubsystem,
17+
BiscuitSubsystem biscuitSubsystem) {
18+
this.robotState = robotState;
19+
addRequirements(elevatorSubsystem, coralSubsystem, biscuitSubsystem);
20+
}
21+
22+
@Override
23+
public void initialize() {
24+
robotState.toStow();
25+
}
26+
27+
@Override
28+
public boolean isFinished() {
29+
return robotState.getState() == RobotStates.STOW;
30+
}
31+
}

‎src/main/java/frc/robot/constants/AlgaeConstants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@
1818
import edu.wpi.first.units.measure.AngularVelocity;
1919

2020
public class AlgaeConstants {
21-
public static int kFxId = 30;
21+
public static final int kFxId = 30;
2222

2323
public static final AngularVelocity kCloseEnough = RotationsPerSecond.of(0.1);
2424
public static final AngularVelocity kMaxFwd = RotationsPerSecond.of(100);

‎src/main/java/frc/robot/constants/ElevatorConstants.java‎

Lines changed: 7 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@
2020

2121
public class ElevatorConstants {
2222

23-
public static final double kCloseEnoughRotations = 0.0083;
23+
public static final double kCloseEnoughRotations = 0.1;
2424
public static final double kMaxFwd = 53; // TODO all of these fields need to be filled out
2525
public static final double kMaxRev = 2;
2626
public static final int kZeroMultiple =
@@ -39,9 +39,8 @@ public class ElevatorConstants {
3939

4040
// Setpoints
4141
// Idle
42-
public static final Angle kFunnelSetpoint = Rotations.of(2.40430);
42+
public static final Angle kFunnelSetpoint = Rotations.of(2.03125); // was 2.40430
4343
public static final Angle kStowSetpoint = kFunnelSetpoint;
44-
public static final Angle kPrestageSetpoint = Rotations.of(0.0);
4544

4645
// Algae removal
4746
public static final Angle kL2AlgaeSetpoint = Rotations.of(0.0);
@@ -55,10 +54,13 @@ public class ElevatorConstants {
5554

5655
// Coral score
5756
public static final Angle kL1CoralSetpoint = Rotations.of(13.04053);
58-
public static final Angle kL2CoralSetpoint = Rotations.of(19.62793);
59-
public static final Angle kL3CoralSetpoint = Rotations.of(30.42969);
57+
public static final Angle kL2CoralSetpoint = Rotations.of(21.0786); // 19.62793 -> 21.0786
58+
public static final Angle kL3CoralSetpoint =
59+
Rotations.of(31.0901); // was 30.42969 -> 31.7505 -> 31.0901
6060
public static final Angle kL4CoralSetpoint = Rotations.of(48.28076);
6161

62+
public static final Angle kPrestageSetpoint = kL3CoralSetpoint;
63+
6264
// Algae obtaining
6365
public static final Angle kFloorAlgaeSetpoint = Rotations.of(0.0);
6466
public static final Angle kMicAlgaeSetpoint = Rotations.of(0.0);

‎src/main/java/frc/robot/constants/FunnelConstants.java‎

Lines changed: 16 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,21 +1,25 @@
11
package frc.robot.constants;
22

3+
import com.ctre.phoenix6.configs.CommutationConfigs;
34
import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
5+
import com.ctre.phoenix6.configs.ExternalFeedbackConfigs;
46
import com.ctre.phoenix6.configs.HardwareLimitSwitchConfigs;
57
import com.ctre.phoenix6.configs.MotorOutputConfigs;
68
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
79
import com.ctre.phoenix6.configs.TalonFXSConfiguration;
10+
import com.ctre.phoenix6.signals.ExternalFeedbackSensorSourceValue;
811
import com.ctre.phoenix6.signals.ForwardLimitSourceValue;
912
import com.ctre.phoenix6.signals.ForwardLimitTypeValue;
13+
import com.ctre.phoenix6.signals.MotorArrangementValue;
1014
import com.ctre.phoenix6.signals.NeutralModeValue;
1115
import com.ctre.phoenix6.signals.ReverseLimitSourceValue;
1216
import com.ctre.phoenix6.signals.ReverseLimitTypeValue;
1317

1418
public class FunnelConstants {
15-
public static final double kFunnelPercentOutput = 0;
19+
public static final double kFunnelPercentOutput = 0.5;
1620

17-
public static int FunnelFxsId = 0;
18-
public static final int kFunnelBeamCounts = 3;
21+
public static int FunnelFxsId = 40;
22+
public static final int kFunnelBeamCounts = 1;
1923

2024
public static TalonFXSConfiguration getFXSConfig() {
2125
TalonFXSConfiguration fxsConfig = new TalonFXSConfiguration();
@@ -55,6 +59,15 @@ public static TalonFXSConfiguration getFXSConfig() {
5559
.withNeutralMode(NeutralModeValue.Coast);
5660
fxsConfig.MotorOutput = motorOut;
5761

62+
CommutationConfigs commutation =
63+
new CommutationConfigs().withMotorArrangement(MotorArrangementValue.Minion_JST);
64+
fxsConfig.Commutation = commutation;
65+
66+
ExternalFeedbackConfigs external =
67+
new ExternalFeedbackConfigs()
68+
.withExternalFeedbackSensorSource(ExternalFeedbackSensorSourceValue.Commutation);
69+
fxsConfig.ExternalFeedback = external;
70+
5871
return fxsConfig;
5972
}
6073
}

‎src/main/java/frc/robot/constants/RobotConstants.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,5 +4,6 @@ public class RobotConstants {
44
public static final int kTalonConfigTimeout = 10; // ms
55

66
public static final double kJoystickDeadband = 0.1;
7+
public static final double kTriggerDeadband = 0.5;
78
public static final double kTestingDeadband = 0.5;
89
}

‎src/main/java/frc/robot/constants/RobotStateConstants.java‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,4 +6,6 @@ public class RobotStateConstants {
66

77
public static final double kRedBargeSafeX = 9.5;
88
public static final double kBlueBargeSafeX = 8;
9+
10+
public static final double kCoralEjectTimer = 0.5;
911
}

0 commit comments

Comments
 (0)