Skip to content

Commit 54725d2

Browse files
rafaelbairdrafaelbaird
committed
(11/27/25) Added Arm subsystem, without visualization
Added arm folder and rewrote Arm.java from Riptide into an IO Structure. Not sure if I wrote ArmIOSim.java correctly. I also have to add visualization to ArmIOSim.java.
1 parent 9ebd13a commit 54725d2

24 files changed

+1092
-2126
lines changed

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

Lines changed: 51 additions & 130 deletions
Original file line numberDiff line numberDiff line change
@@ -14,23 +14,14 @@
1414
package frc.robot;
1515

1616
import com.pathplanner.lib.auto.AutoBuilder;
17-
import edu.wpi.first.math.geometry.Pose2d;
18-
import edu.wpi.first.math.geometry.Rotation2d;
1917
import edu.wpi.first.wpilibj.GenericHID;
2018
import edu.wpi.first.wpilibj.XboxController;
2119
import edu.wpi.first.wpilibj2.command.Command;
22-
import edu.wpi.first.wpilibj2.command.Commands;
2320
import edu.wpi.first.wpilibj2.command.button.CommandPS5Controller;
2421
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
25-
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
26-
import frc.robot.commands.DriveCommands;
27-
import frc.robot.generated.TunerConstants;
28-
import frc.robot.subsystems.drive.Drive;
29-
import frc.robot.subsystems.drive.GyroIO;
30-
import frc.robot.subsystems.drive.GyroIOPigeon2;
31-
import frc.robot.subsystems.drive.ModuleIO;
32-
import frc.robot.subsystems.drive.ModuleIOSim;
33-
import frc.robot.subsystems.drive.ModuleIOTalonFX;
22+
import frc.robot.subsystems.elevator.Elevator;
23+
import frc.robot.subsystems.elevator.ElevatorIOSim;
24+
import frc.robot.subsystems.elevator.ElevatorIOTalonFX;
3425
import frc.robot.subsystems.rollers.Rollers;
3526
import frc.robot.subsystems.rollers.RollersIOSim;
3627
import frc.robot.subsystems.rollers.RollersIOSparkMax;
@@ -43,132 +34,62 @@
4334
* subsystems, commands, and button mappings) should be declared here.
4435
*/
4536
public class RobotContainer {
46-
// Subsystems
47-
private final Drive drive;
48-
public final Rollers rollers;
37+
// Subsystems
38+
public final Rollers rollers;
39+
public final Elevator elevator;
4940

50-
// Controller
51-
public CommandXboxController controller = new CommandXboxController(0);
52-
public static CommandPS5Controller operatorController = new CommandPS5Controller(1);
41+
// Controller
42+
public CommandXboxController controller = new CommandXboxController(0);
43+
public static CommandPS5Controller operatorController = new CommandPS5Controller(1);
5344

54-
// Dashboard inputs
55-
private final LoggedDashboardChooser<Command> autoChooser;
45+
// Dashboard inputs
46+
private final LoggedDashboardChooser<Command> autoChooser;
5647

57-
/** The container for the robot. Contains subsystems, OI devices, and commands. */
58-
public RobotContainer() {
59-
switch (Constants.currentMode) {
60-
case REAL:
61-
// Real robot, instantiate hardware IO implementations
62-
drive =
63-
new Drive(
64-
new GyroIOPigeon2(),
65-
new ModuleIOTalonFX(TunerConstants.FrontLeft),
66-
new ModuleIOTalonFX(TunerConstants.FrontRight),
67-
new ModuleIOTalonFX(TunerConstants.BackLeft),
68-
new ModuleIOTalonFX(TunerConstants.BackRight));
48+
/** The container for the robot. Contains subsystems, OI devices, and commands. */
49+
public RobotContainer() {
50+
switch (Constants.currentMode) {
51+
case REAL:
52+
// Real robot, instantiate hardware IO implementations
53+
rollers = new Rollers(new RollersIOSparkMax());
54+
elevator = new Elevator(new ElevatorIOTalonFX());
6955

70-
rollers = new Rollers(new RollersIOSparkMax());
56+
break;
7157

72-
break;
58+
case SIM:
59+
// Sim robot, instantiate physics sim IO implementations
60+
rollers = new Rollers(new RollersIOSim());
61+
elevator = new Elevator(new ElevatorIOSim());
7362

74-
case SIM:
75-
// Sim robot, instantiate physics sim IO implementations
76-
drive =
77-
new Drive(
78-
new GyroIO() {},
79-
new ModuleIOSim(TunerConstants.FrontLeft),
80-
new ModuleIOSim(TunerConstants.FrontRight),
81-
new ModuleIOSim(TunerConstants.BackLeft),
82-
new ModuleIOSim(TunerConstants.BackRight));
63+
break;
8364

84-
rollers = new Rollers(new RollersIOSim());
65+
default:
66+
// Replayed robot, disable IO implementations
67+
rollers = new Rollers(null);
68+
elevator = new Elevator(null);
8569

86-
break;
87-
88-
default:
89-
// Replayed robot, disable IO implementations
90-
drive =
91-
new Drive(
92-
new GyroIO() {},
93-
new ModuleIO() {},
94-
new ModuleIO() {},
95-
new ModuleIO() {},
96-
new ModuleIO() {});
97-
98-
rollers = new Rollers(null);
99-
100-
break;
101-
}
102-
103-
// Set up auto routines
104-
autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
105-
106-
// Set up SysId routines
107-
autoChooser.addOption(
108-
"Drive Wheel Radius Characterization", DriveCommands.wheelRadiusCharacterization(drive));
109-
autoChooser.addOption(
110-
"Drive Simple FF Characterization", DriveCommands.feedforwardCharacterization(drive));
111-
autoChooser.addOption(
112-
"Drive SysId (Quasistatic Forward)",
113-
drive.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
114-
autoChooser.addOption(
115-
"Drive SysId (Quasistatic Reverse)",
116-
drive.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
117-
autoChooser.addOption(
118-
"Drive SysId (Dynamic Forward)", drive.sysIdDynamic(SysIdRoutine.Direction.kForward));
119-
autoChooser.addOption(
120-
"Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse));
121-
122-
// Configure the button bindings
123-
configureButtonBindings();
70+
break;
12471
}
12572

126-
/**
127-
* Use this method to define your button->command mappings. Buttons can be created by
128-
* instantiating a {@link GenericHID} or one of its subclasses ({@link
129-
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
130-
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
131-
*/
132-
private void configureButtonBindings() {
133-
// Default command, normal field-relative drive
134-
drive.setDefaultCommand(
135-
DriveCommands.joystickDrive(
136-
drive,
137-
() -> -controller.getLeftY(),
138-
() -> -controller.getLeftX(),
139-
() -> -controller.getRightX()));
140-
141-
// Lock to 0° when A button is held
142-
controller
143-
.a()
144-
.whileTrue(
145-
DriveCommands.joystickDriveAtAngle(
146-
drive,
147-
() -> -controller.getLeftY(),
148-
() -> -controller.getLeftX(),
149-
() -> new Rotation2d()));
150-
151-
// Switch to X pattern when X button is pressed
152-
controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
153-
154-
// Reset gyro to 0° when B button is pressed
155-
controller
156-
.b()
157-
.onTrue(
158-
Commands.runOnce(
159-
() ->
160-
drive.setPose(
161-
new Pose2d(drive.getPose().getTranslation(), new Rotation2d())),
162-
drive)
163-
.ignoringDisable(true));
164-
}
165-
166-
/**
167-
* Use this to pass the autonomous command to the main {@link Robot} class.
168-
*
169-
* @return the command to run in autonomous
170-
*/
171-
public Command getAutonomousCommand() {
172-
return autoChooser.get();
173-
}
73+
// Set up auto routines
74+
autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
75+
76+
configureButtonBindings();
77+
}
78+
79+
/**
80+
* Use this method to define your button->command mappings. Buttons can be created by
81+
* instantiating a {@link GenericHID} or one of its subclasses ({@link
82+
* edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
83+
* edu.wpi.first.wpilibj2.command.button.JoystickButton}.
84+
*/
85+
private void configureButtonBindings() {}
86+
87+
/**
88+
* Use this to pass the autonomous command to the main {@link Robot} class.
89+
*
90+
* @return the command to run in autonomous
91+
*/
92+
public Command getAutonomousCommand() {
93+
return autoChooser.get();
94+
}
17495
}

0 commit comments

Comments
 (0)