SequentialCommandGroup Question?

Technical Java
1

We are having an issue that I do not totally understand when using a sequential command group.

Our robot has an elevator, a cross slide (horizontal elevator), and an intake pivot subsystem.

We have a sequential command group that is moving each subsystem in order.

What seems to be happening is when the elevator portion is done it starts to drift when the next two commands happen. Once those command finish it returns to hold its setpoint position.

The full code can be found here

LHSPantherbots/2023-Swerve-X at CR-Clean-Up (github.com)

We see this on several of the commands but the one that I am posting the video of is ConeScoreMid

This video is of the full command.

This video is of the same command with the last item in the command group commented out.

As you can see with the longer command there is more drift seen.

The elevator is pretty well balanced with a spring and what appears to be happening is that as soon as it hits is isAtHeight the pid command stops to start the next command. I think the momentum and the spring allow the elevator to keep drifting up. I thought one of two things should happen next. That either this elevator subsystem should go to the interrupted section of the FunctionalCommand or it would restart my default command for that subsystem. Either of those should tell the elevator to hold at the current setpoint.

It seems that the motor is just not providing any power and is just drifting until all of the commands are finished in the SequentalCommandGroup. Once all of the commands are finished it seems to restart the default command which is holding the running a closedLoopElevator function that holds the heightSetpoint, but only after the whole SequentalCommandGroup is finished.

I can go up after that and try to displace the elevator and it holds steady on its position like you would expect with a PID controller.

Is there a way to make the elevator hold position while the remaining commands are running in the sequential command group. What am I missing? I would have thought the subsystems would act independently in a command group like this.

(I should mention I am a mechanical mentor trying to pose as a software guy so use small words and explain like I don’t know what you are talking about)

Any help would be appreciated and I would be more than happy to expand if I need to.

2

You could try launching multiple commands simultaneously they shouldn’t stop each other you could even launch one within the “initialize()” function of the other, second to make debugging easier in robot periodic place

// Debug hook for periodic scheduled commands
SmartDashboard.putData(CommandScheduler.getInstance());

And then in smartdashboard or glass you should see all your scheduled commands it’ll make checking when commands end and start much easier. Hope this helps

3

Command groups hold all of their required subsystems until the whole group is complete. This will prevent the default command from running.

You probably need to schedule a command to hold the elevator, maybe just a new instance of the default command.

1 Like
4

Check out this example to use closed loop on a sparkmax:

With closed loop the PID loop continues to run on the motor controller regardless of what your code is doing. Which looks like how you expect it to work. But you are using the WPILIB PID stuff and when the command ends it stop using the PID loop and the motor is set to a constant value (whatever was last) and just continues on.

Log what your set command is doing on line 2023-Swerve-X/ElevatorSubsystem.java at CR-Clean-Up · LHSPantherbots/2023-Swerve-X · GitHub

I think that will help you figure it out.

I’m guessing you don’t want the command to end and just let the next command interrupt it. Or else create a default command that just operates the PID loop to keep the elevator at the set point.

I hope this helps!

5

Thanks for the feedback.

This looked like what was happening, but I really thought the subsystems were isolated so I thought I had done something wrong, but it is good to know.

This must be why I don’t recall seeing this issue in the past. We have always done the PID velocity control and other position control using the onboard Spark Max PID. I just heard many people having issues using SmartMotion to do the profiled motion control so we chose to use the WPIlib controls. I am very pleased with the smoothness of the motion, just was confused at some of the quirks.

Thanks for all of your help. I have enough information to be dangerous again.

6

I’m a bit late here, but throwing my $0.02 in anyway. @gdefender has it right on the money. Doing PID on the controller would definitely solve the problem.

Thinking through alternate solutions though… Is there a reason the command group needs to be sequential instead of parallel (at least for the elevator/slide steps)? You could add in your “hold position” commands into the group in some way. E.g. make your group be a parallel group containing a “go to point then hold” sequential group for each subsystem

7

The elevator and cross slide movement can likely be parallel, and we will likely move that way to improve speed in the future, but we started off with sequential to avoid any crashes.

We will explore some of the debugging techniques that have been suggested on these replies, but we are likely going to try the following since it is the least change to our current code structure, and I think it will solve our issues based on the comments.

For each item of the sequential group just add .raceWith(holdPosition command) for the other subsystems. In my understanding this should keep all of the subsystems active, but still allow each item in the sequence to end.