Skip to content

Commit 3537a7d

Browse files
PotatoBoyH4PotatoBoyH4
committed
fixed many commands, fixed constants, and many other wonderous things
1 parent 318ede2 commit 3537a7d

28 files changed

+403
-295
lines changed

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

Lines changed: 50 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -24,18 +24,18 @@
2424
import frc.robot.commands.elevator.JogElevatorCommand;
2525
import frc.robot.commands.elevator.SetElevatorPositionCommand;
2626
import frc.robot.commands.elevator.ZeroElevatorCommand;
27-
import frc.robot.commands.robotState.ToggleAlgaeHeightCommand;
28-
import frc.robot.commands.robotState.ToggleAutoCommand;
29-
import frc.robot.commands.robotState.GetAlgaeCommand;
27+
import frc.robot.commands.robotState.FloorAlgaeCommand;
3028
import frc.robot.commands.robotState.HPAlgaeCommand;
31-
import frc.robot.commands.robotState.InteruptAutoCommand;
29+
import frc.robot.commands.robotState.InterruptAutoCommand;
30+
import frc.robot.commands.robotState.ReefCycleCommand;
3231
import frc.robot.commands.robotState.ScoreAlgaeCommand;
33-
import frc.robot.commands.robotState.ScoreLeftCommand;
34-
import frc.robot.commands.robotState.ScoreReefManualCommand;
35-
import frc.robot.commands.robotState.ScoreRightCommand;
32+
import frc.robot.commands.robotState.SetScoreSideRightCommand;
3633
import frc.robot.commands.robotState.SetScoringLevelCommand;
3734
import frc.robot.commands.robotState.StowCommand;
35+
import frc.robot.commands.robotState.ToggleAlgaeHeightCommand;
36+
import frc.robot.commands.robotState.ToggleAutoCommand;
3837
import frc.robot.commands.robotState.ToggleGetAlgaeCommand;
38+
import frc.robot.commands.robotState.setScoreSideLeftCommand;
3939
import frc.robot.constants.BiscuitConstants;
4040
import frc.robot.constants.ElevatorConstants;
4141
import frc.robot.constants.RobotConstants;
@@ -153,7 +153,7 @@ public RobotContainer() {
153153

154154
configureTelemetry();
155155
configureDriverBindings();
156-
configureTestOperatorBindings();
156+
configureOperatorBindings();
157157
}
158158

159159
private void configureTelemetry() {
@@ -171,26 +171,52 @@ private void configureDriverBindings() {
171171
new DriveTeleopCommand(
172172
() -> flysky.getFwd(), () -> flysky.getStr(), () -> flysky.getYaw(), driveSubsystem));
173173

174-
// Reset Gyro Command, stow, and interrupt Auton
174+
// Reset Gyro Command, stow, interrupt Auton, and zero elev
175175
new JoystickButton(driveJoystick, Button.M_SWC.id).onTrue(new ResetGyroCommand(driveSubsystem));
176176
new JoystickButton(driveJoystick, Button.SWD.id)
177-
.onTrue(new StowCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem))
178-
.onFalse(new StowCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
177+
.onTrue(
178+
new StowCommand(
179+
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem))
180+
.onFalse(
181+
new StowCommand(
182+
robotStateSubsystem, elevatorSubsystem, coralSubsystem, biscuitSubsystem));
179183
new JoystickButton(driveJoystick, Button.SWA.id)
180-
.onTrue(new InteruptAutoCommand(robotStateSubsystem))
181-
.onFalse(new InteruptAutoCommand(robotStateSubsystem));
184+
.onTrue(new InterruptAutoCommand(robotStateSubsystem))
185+
.onFalse(new InterruptAutoCommand(robotStateSubsystem));
186+
new JoystickButton(driveJoystick, Button.SWB_UP.id)
187+
.onTrue(new ZeroElevatorCommand(elevatorSubsystem))
188+
.onFalse(new ZeroElevatorCommand(elevatorSubsystem));
189+
new JoystickButton(driveJoystick, Button.SWB_DWN.id)
190+
.onTrue(new ZeroElevatorCommand(elevatorSubsystem))
191+
.onFalse(new ZeroElevatorCommand(elevatorSubsystem));
182192

183193
// other stuff
184194
new JoystickButton(driveJoystick, Button.M_SWH.id)
185-
.onTrue(new ScoreReefManualCommand(robotStateSubsystem, elevatorSubsystem, coralSubsystem));
195+
.onTrue(
196+
new ReefCycleCommand(
197+
robotStateSubsystem,
198+
elevatorSubsystem,
199+
coralSubsystem,
200+
biscuitSubsystem,
201+
algaeSubsystem));
186202
new JoystickButton(driveJoystick, Button.M_SWE.id)
187-
.onTrue(new ScoreAlgaeCommand(robotStateSubsystem));
203+
.onTrue(
204+
new ScoreAlgaeCommand(
205+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
188206
new JoystickButton(driveJoystick, Button.SWF_UP.id)
189-
.onTrue(new GetAlgaeCommand(robotStateSubsystem))
190-
.onFalse(new GetAlgaeCommand(robotStateSubsystem));
207+
.onTrue(
208+
new FloorAlgaeCommand(
209+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
210+
.onFalse(
211+
new FloorAlgaeCommand(
212+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
191213
new JoystickButton(driveJoystick, Button.SWF_DWN.id)
192-
.onTrue(new GetAlgaeCommand(robotStateSubsystem))
193-
.onFalse(new GetAlgaeCommand(robotStateSubsystem));
214+
.onTrue(
215+
new FloorAlgaeCommand(
216+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem))
217+
.onFalse(
218+
new FloorAlgaeCommand(
219+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
194220
}
195221

196222
private void configureOperatorBindings() {
@@ -216,16 +242,18 @@ private void configureOperatorBindings() {
216242
new JoystickButton(xboxController, XboxController.Button.kX.value)
217243
.onTrue(new ToggleGetAlgaeCommand(robotStateSubsystem));
218244
new JoystickButton(xboxController, XboxController.Button.kB.value)
219-
.onTrue(new HPAlgaeCommand(robotStateSubsystem));
245+
.onTrue(
246+
new HPAlgaeCommand(
247+
robotStateSubsystem, elevatorSubsystem, biscuitSubsystem, algaeSubsystem));
220248

221249
// Scoring
222250
new JoystickButton(xboxController, XboxController.Button.kA.value)
223251
.onTrue(new ToggleAutoCommand(robotStateSubsystem));
224252

225253
new JoystickButton(xboxController, XboxController.Button.kRightStick.value)
226-
.onTrue(new ScoreRightCommand(robotStateSubsystem));
254+
.onTrue(new SetScoreSideRightCommand(robotStateSubsystem));
227255
new JoystickButton(xboxController, XboxController.Button.kLeftStick.value)
228-
.onTrue(new ScoreLeftCommand(robotStateSubsystem));
256+
.onTrue(new setScoreSideLeftCommand(robotStateSubsystem));
229257
}
230258

231259
private void configureTestOperatorBindings() {

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

Lines changed: 12 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,17 @@
11
package frc.robot.commands.robotState;
22

3-
import frc.robot.subsystems.robotState.RobotStateSubsystem;
43
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
55

66
public class ClimbCommand extends InstantCommand {
7-
RobotStateSubsystem robotState;
8-
public ClimbCommand(RobotStateSubsystem robotState){
9-
this.robotState = robotState;
10-
}
11-
@Override
12-
public void initialize() {
13-
robotState.toClimb();
14-
}
15-
}
7+
RobotStateSubsystem robotState;
8+
9+
public ClimbCommand(RobotStateSubsystem robotState) {
10+
this.robotState = robotState;
11+
}
12+
13+
@Override
14+
public void initialize() {
15+
robotState.toClimb();
16+
}
17+
}

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

Lines changed: 11 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,17 @@
11
package frc.robot.commands.robotState;
22

3-
import frc.robot.subsystems.robotState.RobotStateSubsystem;
43
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
55

66
public class ClimbPrepCommand extends InstantCommand {
7-
RobotStateSubsystem robotState;
8-
public ClimbPrepCommand(RobotStateSubsystem robotState){
9-
this.robotState = robotState;
10-
}
11-
@Override
12-
public void initialize() {
13-
robotState.toPrepClimb();
14-
}
7+
RobotStateSubsystem robotState;
8+
9+
public ClimbPrepCommand(RobotStateSubsystem robotState) {
10+
this.robotState = robotState;
11+
}
12+
13+
@Override
14+
public void initialize() {
15+
robotState.toPrepClimb();
16+
}
1517
}

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

Lines changed: 32 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,37 @@
11
package frc.robot.commands.robotState;
22

3-
import frc.robot.subsystems.robotState.RobotStateSubsystem;
4-
import edu.wpi.first.wpilibj2.command.InstantCommand;
3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.algae.AlgaeSubsystem;
5+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
6+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
7+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
8+
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
59

6-
public class FloorAlgaeCommand extends InstantCommand {
7-
RobotStateSubsystem robotState;
8-
public FloorAlgaeCommand(RobotStateSubsystem robotState){
9-
this.robotState = robotState;
10-
}
11-
@Override
12-
public void initialize() {
13-
robotState.toAlgaeFloorPickup();
10+
public class FloorAlgaeCommand extends Command {
11+
RobotStateSubsystem robotState;
12+
boolean hasTriedToPickup = false;
13+
14+
public FloorAlgaeCommand(
15+
RobotStateSubsystem robotState,
16+
ElevatorSubsystem elevatorSubsystem,
17+
BiscuitSubsystem biscuitSubsystem,
18+
AlgaeSubsystem algaeSubsystem) {
19+
this.robotState = robotState;
20+
addRequirements(elevatorSubsystem, biscuitSubsystem, algaeSubsystem);
21+
}
22+
23+
@Override
24+
public void initialize() {
25+
hasTriedToPickup = false;
26+
robotState.toAlgaeFloorPickup();
27+
}
28+
29+
@Override
30+
public boolean isFinished() {
31+
if (robotState.getState() == RobotStates.FLOOR_ALGAE
32+
|| robotState.getState() == RobotStates.MIC_ALGAE) {
33+
hasTriedToPickup = true;
1434
}
35+
return robotState.getState() == RobotStates.STOW && hasTriedToPickup;
36+
}
1537
}

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

Lines changed: 15 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,20 +1,21 @@
11
package frc.robot.commands.robotState;
22

3-
import frc.robot.subsystems.robotState.RobotStateSubsystem;
43
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
55

66
public class GetAlgaeCommand extends InstantCommand {
7-
RobotStateSubsystem robotState;
8-
public GetAlgaeCommand(RobotStateSubsystem robotState){
9-
this.robotState = robotState;
10-
}
11-
@Override
12-
public void initialize() {
13-
if(robotState.getGetAlgaeOnCycle() == true) {
14-
robotState.setGetAlgaeOnCycle(false);
15-
}
16-
else{
17-
robotState.setGetAlgaeOnCycle(true);
18-
}
19-
}
7+
RobotStateSubsystem robotState;
8+
9+
public GetAlgaeCommand(RobotStateSubsystem robotState) {
10+
this.robotState = robotState;
11+
}
12+
13+
@Override
14+
public void initialize() {
15+
if (robotState.getGetAlgaeOnCycle() == true) {
16+
robotState.setGetAlgaeOnCycle(false);
17+
} else {
18+
robotState.setGetAlgaeOnCycle(true);
2019
}
20+
}
21+
}

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

Lines changed: 30 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,15 +1,36 @@
11
package frc.robot.commands.robotState;
22

3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import frc.robot.subsystems.algae.AlgaeSubsystem;
5+
import frc.robot.subsystems.biscuit.BiscuitSubsystem;
6+
import frc.robot.subsystems.elevator.ElevatorSubsystem;
37
import frc.robot.subsystems.robotState.RobotStateSubsystem;
4-
import edu.wpi.first.wpilibj2.command.InstantCommand;
8+
import frc.robot.subsystems.robotState.RobotStateSubsystem.RobotStates;
59

6-
public class HPAlgaeCommand extends InstantCommand {
7-
RobotStateSubsystem robotState;
8-
public HPAlgaeCommand(RobotStateSubsystem robotState){
9-
this.robotState = robotState;
10-
}
11-
@Override
12-
public void initialize() {
13-
robotState.toHpAlgae();
10+
public class HPAlgaeCommand extends Command {
11+
RobotStateSubsystem robotState;
12+
private boolean hasTriedToGrab = false;
13+
14+
public HPAlgaeCommand(
15+
RobotStateSubsystem robotState,
16+
ElevatorSubsystem elevatorSubsystem,
17+
BiscuitSubsystem biscuitSubsystem,
18+
AlgaeSubsystem algaeSubsystem) {
19+
this.robotState = robotState;
20+
addRequirements(elevatorSubsystem, biscuitSubsystem, algaeSubsystem);
21+
}
22+
23+
@Override
24+
public void initialize() {
25+
hasTriedToGrab = false;
26+
robotState.toHpAlgae();
27+
}
28+
29+
@Override
30+
public boolean isFinished() {
31+
if (robotState.getState() == RobotStates.HP_ALGAE) {
32+
hasTriedToGrab = true;
1433
}
34+
return robotState.getState() == RobotStates.STOW && hasTriedToGrab;
35+
}
1536
}

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

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
package frc.robot.commands.robotState;
2+
3+
import edu.wpi.first.wpilibj2.command.InstantCommand;
4+
import frc.robot.subsystems.robotState.RobotStateSubsystem;
5+
6+
public class InterruptAutoCommand
7+
extends InstantCommand { // TODO fix this command, it dose not work
8+
RobotStateSubsystem robotState;
9+
10+
public InterruptAutoCommand(RobotStateSubsystem robotState) {
11+
this.robotState = robotState;
12+
}
13+
14+
@Override
15+
public void initialize() {
16+
robotState.toInterrupted();
17+
}
18+
}

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

Lines changed: 0 additions & 17 deletions
This file was deleted.

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

Lines changed: 0 additions & 16 deletions
This file was deleted.

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

Lines changed: 0 additions & 16 deletions
This file was deleted.

0 commit comments

Comments
 (0)