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.