Skip to content

Commit 8e53bec

Browse files
Wu-Fan-529Wu-Fan-529
committed
Button Mapping
1 parent e07710e commit 8e53bec

File tree

5 files changed

+244
-0
lines changed

5 files changed

+244
-0
lines changed

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

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -156,6 +156,12 @@ public static class constDrivetrain {
156156
CANCODER_CONFIG.MagnetSensor.SensorDirection = SensorDirectionValue.CounterClockwise_Positive;
157157
}
158158

159+
public static class constIntake {
160+
// TODO: Replace with actual measurements
161+
public static final Current ALGAE_INTAKE_HAS_GP_CURRENT = Units.Amps.of(15);
162+
public static final AngularVelocity ALGAE_INTAKE_HAS_GP_VELOCITY = Units.RotationsPerSecond.of(2102 / 60);
163+
}
164+
159165
public static class AUTO {
160166
// This PID is implemented on the Drivetrain subsystem
161167
// TODO: AUTO PID

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

Lines changed: 177 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
import edu.wpi.first.math.geometry.Rotation2d;
1111
import edu.wpi.first.wpilibj2.command.Command;
1212
import edu.wpi.first.wpilibj2.command.Commands;
13+
import edu.wpi.first.wpilibj2.command.button.Trigger;
1314
import frc.robot.Constants.constControllers;
1415
import frc.robot.RobotMap.mapControllers;
1516
import frc.robot.commands.*;
@@ -23,6 +24,7 @@
2324
public class RobotContainer {
2425

2526
private final SN_XboxController conDriver = new SN_XboxController(mapControllers.DRIVER_USB);
27+
private final SN_XboxController conOperator = new SN_XboxController(mapControllers.OPERATOR_USB);
2628

2729
private final Drivetrain subDrivetrain = new Drivetrain();
2830
private final Intake subIntake = new Intake();
@@ -31,8 +33,93 @@ public class RobotContainer {
3133
private final StateMachine subStateMachine = new StateMachine(subDrivetrain, subIntake, subClimber, subElevator);
3234
private final RobotPoses robotPose = new RobotPoses(subDrivetrain);
3335

36+
private final Trigger hasCoralTrigger = new Trigger(() -> subIntake.hasCoral() && !subIntake.hasAlgae());
37+
private final Trigger hasAlgaeTrigger = new Trigger(() -> !subIntake.hasCoral() && subIntake.hasAlgae()
38+
&& subStateMachine.getRobotState() != RobotState.SCORING_CORAL_WITH_ALGAE
39+
&& subStateMachine.getRobotState() != RobotState.INTAKE_CORAL_WITH_ALGAE_GROUND);
40+
private final Trigger hasBothTrigger = new Trigger(() -> subIntake.hasCoral() && subIntake.hasAlgae());
41+
3442
Command TRY_NONE = Commands.deferredProxy(
3543
() -> subStateMachine.tryState(RobotState.NONE));
44+
Command TRY_CLIMBING = Commands.deferredProxy(
45+
() -> subStateMachine.tryState(RobotState.CLIMBING));
46+
Command TRY_PREP_CLIMB = Commands.deferredProxy(
47+
() -> subStateMachine.tryState(RobotState.PREP_CLIMB));
48+
Command TRY_RETRACTING_CLIMBER = Commands.deferredProxy(
49+
() -> subStateMachine.tryState(RobotState.RETRACTING_CLIMBER));
50+
Command TRY_PREP_CORAL_ZERO = Commands.deferredProxy(
51+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_ZERO));
52+
Command TRY_PREP_CORAL_L1 = Commands.deferredProxy(
53+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_L1));
54+
Command TRY_PREP_CORAL_L2 = Commands.deferredProxy(
55+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_L2));
56+
Command TRY_PREP_CORAL_L3 = Commands.deferredProxy(
57+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_L3));
58+
Command TRY_PREP_CORAL_L4 = Commands.deferredProxy(
59+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_L4));
60+
Command TRY_PREP_CORAL_WITH_ALGAE_L1 = Commands.deferredProxy(
61+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_WITH_ALGAE_L1));
62+
Command TRY_PREP_CORAL_WITH_ALGAE_L2 = Commands.deferredProxy(
63+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_WITH_ALGAE_L2));
64+
Command TRY_PREP_CORAL_WITH_ALGAE_L3 = Commands.deferredProxy(
65+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_WITH_ALGAE_L3));
66+
Command TRY_PREP_CORAL_WITH_ALGAE_L4 = Commands.deferredProxy(
67+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_WITH_ALGAE_L4));
68+
Command TRY_PREP_CORAL_ZERO_WITH_ALGAE = Commands.deferredProxy(
69+
() -> subStateMachine.tryState(RobotState.PREP_CORAL_ZERO_WITH_ALGAE));
70+
Command TRY_PREP_ALGAE_NET = Commands.deferredProxy(
71+
() -> subStateMachine.tryState(RobotState.PREP_ALGAE_NET));
72+
Command TRY_PREP_ALGAE_PROCESSOR = Commands.deferredProxy(
73+
() -> subStateMachine.tryState(RobotState.PREP_ALGAE_PROCESSOR));
74+
Command TRY_PREP_ALGAE_ZERO = Commands.deferredProxy(
75+
() -> subStateMachine.tryState(RobotState.PREP_ALGAE_ZERO));
76+
Command TRY_PREP_ALGAE_NET_WITH_CORAL = Commands.deferredProxy(
77+
() -> subStateMachine.tryState(RobotState.PREP_ALGAE_NET_WITH_CORAL));
78+
Command TRY_PREP_ALGAE_PROCESSOR_WITH_CORAL = Commands.deferredProxy(
79+
() -> subStateMachine.tryState(RobotState.PREP_ALGAE_PROCESSOR_WITH_CORAL));
80+
Command TRY_PREP_ALGAE_ZERO_WITH_CORAL = Commands.deferredProxy(
81+
() -> subStateMachine.tryState(RobotState.PREP_ALGAE_ZERO_WITH_CORAL));
82+
Command TRY_HAS_CORAL = Commands.deferredProxy(
83+
() -> subStateMachine.tryState(RobotState.HAS_CORAL));
84+
Command TRY_HAS_ALGAE = Commands.deferredProxy(
85+
() -> subStateMachine.tryState(RobotState.HAS_ALGAE));
86+
Command TRY_HAS_CORAL_AND_ALGAE = Commands.deferredProxy(
87+
() -> subStateMachine.tryState(RobotState.HAS_CORAL_AND_ALGAE));
88+
Command TRY_SCORING_CORAL = Commands.deferredProxy(
89+
() -> subStateMachine.tryState(RobotState.SCORING_CORAL));
90+
Command TRY_SCORING_ALGAE = Commands.deferredProxy(
91+
() -> subStateMachine.tryState(RobotState.SCORING_ALGAE));
92+
Command TRY_CLEAN_HIGH = Commands.deferredProxy(
93+
() -> subStateMachine.tryState(RobotState.CLEAN_HIGH));
94+
Command TRY_CLEAN_LOW = Commands.deferredProxy(
95+
() -> subStateMachine.tryState(RobotState.CLEAN_LOW));
96+
Command TRY_INTAKE_CORAL_STATION = Commands.deferredProxy(
97+
() -> subStateMachine.tryState(RobotState.INTAKE_CORAL_STATION));
98+
Command TRY_INTAKE_ALGAE_GROUND = Commands.deferredProxy(
99+
() -> subStateMachine.tryState(RobotState.INTAKE_ALGAE_GROUND));
100+
Command TRY_EJECTING = Commands.deferredProxy(
101+
() -> subStateMachine.tryState(RobotState.EJECTING));
102+
Command TRY_SCORING_ALGAE_WITH_CORAL = Commands.deferredProxy(
103+
() -> subStateMachine.tryState(RobotState.SCORING_ALGAE_WITH_CORAL));
104+
Command TRY_SCORING_CORAL_WITH_ALGAE = Commands.deferredProxy(
105+
() -> subStateMachine.tryState(RobotState.SCORING_CORAL_WITH_ALGAE));
106+
Command TRY_CLEAN_HIGH_WITH_CORAL = Commands.deferredProxy(
107+
() -> subStateMachine.tryState(RobotState.CLEAN_HIGH_WITH_CORAL));
108+
Command TRY_CLEAN_LOW_WITH_CORAL = Commands.deferredProxy(
109+
() -> subStateMachine.tryState(RobotState.CLEAN_LOW_WITH_CORAL));
110+
Command TRY_INTAKE_CORAL_GROUND = Commands.deferredProxy(
111+
() -> subStateMachine.tryState(RobotState.INTAKE_CORAL_GROUND));
112+
Command TRY_INTAKE_CORAL_WITH_ALGAE_GROUND = Commands.deferredProxy(
113+
() -> subStateMachine.tryState(RobotState.INTAKE_CORAL_WITH_ALGAE_GROUND));
114+
Command TRY_INTAKE_ALGAE_WITH_CORAL_GROUND = Commands.deferredProxy(
115+
() -> subStateMachine.tryState(RobotState.INTAKE_ALGAE_WITH_CORAL_GROUND));
116+
Command HAS_CORAL_OVERRIDE = Commands.deferredProxy(
117+
() -> subStateMachine.tryCoralOverride());
118+
Command HAS_ALGAE_OVERRIDE = Commands.runOnce(() -> subIntake.algaeToggle());
119+
Command TRY_NONE_FROM_SCORING = Commands.deferredProxy(
120+
() -> subStateMachine.tryState(RobotState.NONE)
121+
.unless(() -> (subStateMachine.getRobotState() == RobotState.SCORING_CORAL
122+
|| subStateMachine.getRobotState() == RobotState.SCORING_CORAL_WITH_ALGAE)));
36123

37124
public RobotContainer() {
38125
conDriver.setLeftDeadband(constControllers.DRIVER_LEFT_STICK_DEADBAND);
@@ -57,6 +144,15 @@ private void configDriverBindings() {
57144
conDriver.btn_LeftBumper
58145
.whileTrue(Commands.runOnce(() -> subDrivetrain.setRobotRelative()))
59146
.onFalse(Commands.runOnce(() -> subDrivetrain.setFieldRelative()));
147+
148+
conDriver.btn_Start
149+
.onTrue(TRY_PREP_CLIMB);
150+
151+
conDriver.btn_A
152+
.whileTrue(TRY_RETRACTING_CLIMBER);
153+
154+
conDriver.btn_Y
155+
.whileTrue(TRY_CLIMBING);
60156
}
61157

62158
public Command getAutonomousCommand() {
@@ -66,6 +162,87 @@ public Command getAutonomousCommand() {
66162

67163
private void configOperatorBindings() {
68164
// Add operator bindings here if needed
165+
conOperator.btn_LeftTrigger
166+
.whileTrue(TRY_INTAKE_CORAL_GROUND)
167+
.whileTrue(TRY_INTAKE_CORAL_WITH_ALGAE_GROUND)
168+
.onFalse(TRY_NONE)
169+
.onFalse(TRY_HAS_ALGAE);
170+
171+
conOperator.btn_LeftBumper
172+
.whileTrue(TRY_INTAKE_ALGAE_GROUND)
173+
.whileTrue(TRY_INTAKE_ALGAE_WITH_CORAL_GROUND)
174+
.onFalse(TRY_NONE)
175+
.onFalse(TRY_HAS_CORAL);
176+
177+
conOperator.btn_RightTrigger
178+
.whileTrue(TRY_SCORING_CORAL)
179+
.whileTrue(TRY_SCORING_ALGAE)
180+
.whileTrue(TRY_SCORING_ALGAE_WITH_CORAL)
181+
.whileTrue(TRY_SCORING_CORAL_WITH_ALGAE)
182+
.onFalse(TRY_NONE_FROM_SCORING)
183+
.onFalse(TRY_HAS_CORAL);
184+
185+
conOperator.btn_RightBumper
186+
.whileTrue(TRY_INTAKE_CORAL_STATION)
187+
.onFalse(TRY_NONE)
188+
.onFalse(TRY_HAS_ALGAE);
189+
190+
conOperator.btn_A
191+
.onTrue(TRY_PREP_CORAL_L1)
192+
.onTrue(TRY_PREP_CORAL_WITH_ALGAE_L1);
193+
194+
conOperator.btn_B
195+
.onTrue(TRY_PREP_CORAL_L3)
196+
.onTrue(TRY_PREP_CORAL_WITH_ALGAE_L3);
197+
198+
conOperator.btn_X
199+
.onTrue(TRY_PREP_CORAL_L2)
200+
.onTrue(TRY_PREP_CORAL_WITH_ALGAE_L2);
201+
202+
conOperator.btn_Y
203+
.onTrue(TRY_PREP_CORAL_L4)
204+
.onTrue(TRY_PREP_CORAL_WITH_ALGAE_L4);
205+
206+
conOperator.btn_LeftStick
207+
.onTrue(TRY_PREP_CORAL_ZERO)
208+
.onTrue(TRY_PREP_CORAL_ZERO_WITH_ALGAE);
209+
210+
conOperator.btn_RightStick
211+
.whileTrue(TRY_EJECTING)
212+
.onFalse(TRY_NONE);
213+
214+
conOperator.btn_North
215+
.onTrue(TRY_PREP_ALGAE_NET)
216+
.onTrue(TRY_PREP_ALGAE_NET_WITH_CORAL);
217+
218+
conOperator.btn_South
219+
.onTrue(TRY_PREP_ALGAE_PROCESSOR)
220+
.onTrue(TRY_PREP_ALGAE_PROCESSOR_WITH_CORAL);
221+
222+
conOperator.btn_East
223+
.whileTrue(TRY_CLEAN_HIGH)
224+
.whileTrue(TRY_CLEAN_HIGH_WITH_CORAL)
225+
.onFalse(TRY_NONE)
226+
.onFalse(TRY_HAS_CORAL);
227+
228+
conOperator.btn_West
229+
.whileTrue(TRY_CLEAN_LOW)
230+
.whileTrue(TRY_CLEAN_LOW_WITH_CORAL)
231+
.onFalse(TRY_NONE)
232+
.onFalse(TRY_HAS_CORAL);
233+
234+
conOperator.btn_Start.onTrue(HAS_CORAL_OVERRIDE);
235+
236+
conOperator.btn_Back.onTrue(HAS_ALGAE_OVERRIDE);
237+
238+
hasCoralTrigger
239+
.whileTrue(TRY_HAS_CORAL);
240+
241+
hasAlgaeTrigger
242+
.whileTrue(TRY_HAS_ALGAE);
243+
244+
hasBothTrigger
245+
.whileTrue(TRY_HAS_CORAL_AND_ALGAE);
69246
}
70247

71248
public RobotState getRobotState() {

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
public class RobotMap {
66
public static class mapControllers {
77
public static final int DRIVER_USB = 0;
8+
public static final int OPERATOR_USB = 1;
89
}
910

1011
public static class mapDrivetrain {

‎src/main/java/frc/robot/subsystems/Intake.java‎

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,14 +6,19 @@
66

77
import com.ctre.phoenix6.hardware.TalonFX;
88

9+
import edu.wpi.first.units.measure.AngularVelocity;
10+
import edu.wpi.first.units.measure.Current;
911
import edu.wpi.first.wpilibj2.command.SubsystemBase;
12+
import frc.robot.Constants.constDrivetrain.constIntake;
1013
import frc.robot.RobotMap.mapIntake;
1114

1215
public class Intake extends SubsystemBase {
1316
TalonFX intakePivotMotor;
1417
TalonFX coralLeftMotor;
1518
TalonFX coralRightMotor;
1619
TalonFX algaeIntakeMotor;
20+
public boolean hasCoral = false;
21+
public boolean hasAlgaeOverride = false;
1722

1823
/** Creates a new Intake. */
1924
public Intake() {
@@ -26,6 +31,48 @@ public Intake() {
2631
// e.g., intakePivotMotor.configFactoryDefault();
2732
}
2833

34+
public boolean hasCoral() {
35+
return hasCoral;
36+
}
37+
38+
public void setHasCoral(boolean hasCoral) {
39+
this.hasCoral = hasCoral;
40+
}
41+
42+
public void coralToggle() {
43+
this.hasCoral = !hasCoral;
44+
}
45+
46+
public boolean hasAlgae() {
47+
Current intakeCurrent = algaeIntakeMotor.getStatorCurrent().getValue();
48+
49+
AngularVelocity intakeVelocity = algaeIntakeMotor.getVelocity().getValue();
50+
double intakeAcceleration = algaeIntakeMotor.getAcceleration().getValueAsDouble();
51+
52+
Current intakeHasGamePieceCurrent = constIntake.ALGAE_INTAKE_HAS_GP_CURRENT;
53+
AngularVelocity intakeHasGamePieceVelocity = constIntake.ALGAE_INTAKE_HAS_GP_VELOCITY;
54+
55+
if (hasAlgaeOverride) {
56+
return hasAlgaeOverride;
57+
}
58+
59+
if ((intakeCurrent.gte(intakeHasGamePieceCurrent))
60+
&& (intakeVelocity.lte(intakeHasGamePieceVelocity))
61+
&& (intakeAcceleration < 0)) {
62+
return true;
63+
} else {
64+
return false;
65+
}
66+
}
67+
68+
public void setHasAlgaeOverride(boolean passedHasGamePiece) {
69+
hasAlgaeOverride = passedHasGamePiece;
70+
}
71+
72+
public void algaeToggle() {
73+
this.hasAlgaeOverride = !hasAlgaeOverride;
74+
}
75+
2976
@Override
3077
public void periodic() {
3178
// This method will be called once per scheduler run

‎src/main/java/frc/robot/subsystems/StateMachine.java‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,19 @@ public void setDriverState(DriverState driverState) {
6565
currentDriverState = driverState;
6666
}
6767

68+
public Command tryCoralOverride() {
69+
RobotState[] goToHasBothStates = { RobotState.HAS_ALGAE, RobotState.PREP_ALGAE_PROCESSOR, RobotState.PREP_ALGAE_NET,
70+
RobotState.INTAKE_CORAL_WITH_ALGAE_GROUND };
71+
72+
subIntake.setHasCoral(true);
73+
for (RobotState state : goToHasBothStates) {
74+
if (currentRobotState == state) {
75+
return new HasCoralAndAlgae(subStateMachine);
76+
}
77+
}
78+
return new HasCoral(subStateMachine);
79+
}
80+
6881
public Command tryState(RobotState desiredState) {
6982
switch (desiredState) {
7083
case NONE:

0 commit comments

Comments
 (0)