Hi everyone, this is my first year doing programming, and I am the sole programmer on our team. I have been a mechanical member in previous years however I was forced to step up to this role due to a lack of programmers. Right now I have programmed the wrist and arm to move seperately but noticed that there are more setpoints then buttons. So what I want to do is have it so when I press one button both the arm and wrist moves to the correct setpoint at the same time. However, I have no idea how to do that and need help. This is the github. Here is a picture of the CAD of the robot if its helpful:
This is my first year doing programming so I’m new to this as well. However I’m fairly sure that you need to create a separate command with a parallel command group to get both subsystems to move at the same time. Don’t quote me on this though.
@ImpactTang is correct, what you’d want is a command group, or as they are calling it in more recent version of WPILIB Command Compositions. Check out the sequence and parallel sections on the page below for more information and some simple code examples.
public class ConeSubstationCommand extends CommandBase {
public ConeSubstationCommand(WristSubsystem wrist, ArmSubsystem arm){
addCommands(
new WristSubsystem(wristSubstation),
new ArmSubsystem(armSubstation)
);
}
A command group is made up of other commands. For the old wpilib method, you can make the command extend CommandGroup (specifically ParallelCommandGroup) and you’d use addCommands() to add individual commands that make up the group. For example:
public ConeSubstationCommand extends ParallelCommandGroup {
public ConeSubStationCommand() {
addCommands( new ArmCommand(), new WristCommand());
}
}
ArmCommand and WristCommand being drop-ins for whatever commands you’re trying to use
With Command Groups/Command Compositions you still create the individual commands to use in the Group/Composition. So you’d have a “Wrist” command and an “Arm” command, and you use the Group/Composition to execute them on the same “Trigger” either in parallel or sequentially depending on what is safe for your mechanism design.
So what you wrote above should work, or something like one of the following directly in your RobotContainer:
So to keep it high level. At minimum you should have some kind of PID Controller object in your Subsystem (be it through a CAN Motor Controller or the WPILIB native one) and some method to set a setpoint for the Subsystem’s PID Controller.
Then you need Command(s) to set the setpoint for the Subsystem’s PID Controller. You can create many Commands, one for each setpoint, but the way I do this personally is to write a Command class where I pass in the new setpoint so I can use the same single Command for all my PID setSetpoint stuff.
The tricky bit usually comes on how you want to handle “ending” your commands. Some people have commands end instantly after setting the setpoint in the subsystem, others use a static timeout and assume their mechanism will reach the position in time, but personally I go for the “check the sensor” approach. Adding a “Current Position” or “At Setpoint” methods to my Subsystem and checking against that in my Command’s end() method, applying my allowable error and such in this piece of logic (ie. abs(currentPosition - setPoint) <= allowedError).
I also usually pass my new set point to my commands as a DoubleSupplier so in addition to static setpoints, I can also hook it into a “Set Point feed” like from a camera, other sensor, joystick, and so on where the setpoint may change as the robot, an object, the goal, etc. moves.
Here is an example I wrote last year when I was also trying out a new “paradigm” of defining the commands in the subsystem as well. Check out the RearArm and ForeArm Subsystems for PID stuff, and then the RobotContainer for composing those commands together.
You can do this in RobotContainer, or in some other intermediate scope that holds both subsystems (you could call this a “system” or “supersystem”, but it doesn’t really need any particular abstract class representation since it just serves as a scope).
Consider removing the stuttering from your method names, since they’re instance methods and if your instance is descriptively named you don’t need to repeat it.
