HELP! CommandGroup not working

[jfitz0807]

We're at competition. We had a number of autonomous command groups coded, but we didn't get to test them on the competition bot until this morning. We're using the SmartDashboard chooser to select one for each match. At least, that was the plan.

It turns out that no matter what we try, even defining just a single commandGroup and not using the chooser, our robot runs through autonomousInit() and then goes into autonomousPeriodic(), but it never returns form the call to Scheduler::GetInstance()->Run(); We can't figure out what's going wrong. A printf right after the call never appears in the log, but the code is still running. The DriverStation still shows "Robot Code" green and we can still transition into and out of Teleop and even back into Auto. It's just that nothing happens on the robot. None of our Teleop commands from the joystick work and our camera feed doesn't work either.

If we change our autonomousCommand from a CommandGroup to a Command, it works.

Full disclosure: We used java last year and switched to C++ this year since we had more C++ mentors and our java programmers graduated last year. Since we didn't use C++ last year, we don't have a CommandBase class from which our Commands inherit. According to the documentation, this is not necessary this year.

I'm going to try to make a number of commands that explicitly chain other commands together by having one call the next it it's end() method as a workaround, but if anyone has any thoughts on how to make the CommandGroups work, that would be greatly appreciated.

[otherguy]

Can you post your code? At a minimum Robot.cpp?

[jfitz0807]

Here are some code snippets:

In Robot.h:

class Robot : public IterativeRobot {
private:

public:
CommandGroup *autonomousCommand;
SendableChooser *chooser;
CommandGroup *autoCommand1Can;
CommandGroup *autoCommandMoveToZone;
CommandGroup *autoCommand1Can1Tote;
CommandGroup *autoCommandDoNothing;

Robot.cpp:

autoCommand1Can = new AutonomousCommand1Can();
autoCommandMoveToZone = new AutonomousMoveToZone();
autoCommand1Can1Tote = new AutonomousCommand1Can1Tote();
autoCommandDoNothing = new AutonomousEmpty();

// Stuff to get autonomous selection on SmartDashboard
chooser = new SendableChooser();
chooser->AddDefault("Can to Auto Zone", autoCommandMoveToZone);
chooser->AddObject("Drive to Auto Zone", autoCommand1Can);
chooser->AddObject("Drive to Auto Zone", autoCommand1Can1Tote);
chooser->AddObject("Do absolutely nothing", autoCommandDoNothing);
SmartDashboard:utData("Autonomous Modes",chooser);


Each of the commands is a CommandGroup in which we addSequential a series or other commands, each of which has been tested.

Thanks again for any help.

[EmileH]

I have a feeling that your autonomous CommandGroup isn't ending. Try putting in a parallel waitcommand that goes for 14 seconds (or the length of your autonomous, whichever is smaller, because you want to leave time for the robot to kick back into the default autonomous).

[cbf]

You're welcome to all of it: https://github.com/ligerbots/Recycle...ree/dartFriday

Specifically, if look at line 149 of Robot.cpp in AutonomousInit(). You'll see a commented out line:

autonomousCommand = (CommandGroup *)new DrivePID(100.0, 100.0);

Note that DrivePID is a command (i.e. derives from the WPILib Command class). The assignment above shouldn't work. But it does. It's the only thing we've gotten to work in autonomous -- running a single command.

When we assign a class that instead derives from CommandGroup (so we can do things like AddSequential and AddParallel), which is how we thought this was supposed to work the first call to Scheduler::GetInstance()->Run(); in AutonomousPeriodic goes away and never returns. I traced a couple of instruction into it, and it definitely looks like its been passed a class other than what it expects and is thus calling through a random vtable pointer. We don't get a segfault, but nothing in the program runs after that.

The code at that Git repository right now is the code that fails (but that might change if I can find a fix tonight).

John and I have been trying to find examples of a working Iterative Robot C++ autonomous without success so far, but we'd be very happy if someone can look at our code and tell us we've done thing stupid and obvious..

[otherguy]

I don't see a problem with the excerpts provided. If you could provide a more complete set of code that would likely prove useful. (like the a command group and the commands which it's running)


Have you tried running one of the command groups that isn't working in auto mode from a joystick button.

Map the command group to a button in the OI class, enable in teleop mode. Press the button. If if you have the same problem, you have an issue in your command group or in one of the commands which it's calling. If you don't then there's something wrong with the way you're using the scheduler?

I can only guess at this point since I don't have a clear picture of what you're doing in your code.

[jfitz0807]

Here's our autonomousPeriodic():

void Robot::AutonomousPeriodic() {
if (autoPeriodicCount++ < 4) {
printf("AutoPeriodic %d!\n", autoPeriodicCount);
}
else if (autoPeriodicCount==120) printf("AutoPeriodic still alive %d!\n", autoPeriodicCount);
Scheduler::GetInstance()->Run();
UpdateDashboardPeriodic();
Camera::Feed();
}

We only see the printf once. And the UpdateDashboardPeriodic and Camera::Feed are not getting called. The dashboard does not update and we don't get a camera image. Plus there are printfs in the Camera class that we don't see.

It looks like Scheduler::GetInstance()->Run() never returns, but we don;t get error messages to the console, riolog or even to the terminal if we explicitly run the FRCUserProgram from the linux shell. And it doesn't segfault either.

[jfitz0807]

Our mechanical team is currently working some repairs so I can't try executing a commandGroup from a button. I will try as soon as practicable. That's a great suggestion.

Charles (CBF) has our Test Bot at home. I am still at the venue.

[otherguy]

For you auto modes, all the addSequential calls that you're making within the robotinit function should be made directly within the CommandGroup classes that you have in the Command package.

The commandgroup comments in wpilib for the addsequential function state that calls should be made in the constructor of the CommandGroup. I haven't gone through the source code to see why exactly, but if for nothing else it helps keep your code more organized/readable.

[cbf]

Originally Posted by emileh3467

I have a feeling that your autonomous CommandGroup isn't ending. Try putting in a parallel waitcommand that goes for 14 seconds (or the length of your autonomous, whichever is smaller, because you want to leave time for the robot to kick back into the default autonomous).

Don't think so. None of the commands in our command groups seem to get run at all. Look at https://github.com/ligerbots/Recycle...ands/Delay.cpp -- a command that simply counts off a number of ticks and returns. We never see the output from the printf in the initialization method.

As I said above, the Scheduler::GetInstance()->Run(); in AutonomousPeriodic executes exactly once and never returns..

[jfitz0807]

We had tried that first, but it was giving the same behavior. We were grasping at straws. The Orange Hat guys here thought it was worth a try so we gave it a go.

[Kevin Sevcik]

At least one thing you're doing wrong is reusing Commands all over the place in your CommandGroups and such. For instance:

AddSequential(Robot::oi->canUp);

That's wrong. You can't reuse Commands like this, especially in CommandGroups. Here's the relevant code from WPILib:

void Command::Start()
{
	LockChanges();
	if (m_parent != NULL)
		wpi_setWPIErrorWithContext(CommandIllegalUse, "Can not start a command that is part of a command group");

	Scheduler::GetInstance()->AddCommand(this);
}

So once you call that CommandGroup constructor with that bad AddSequential, the command instance for your CanUp button gets a parent added to it and will no longer work because that instance now thinks it's part of a CommandGroup. So everywhere you have this design anti-pattern:

AddSequential(Robot::oi->canUp);

You should replace with something like:

AddSequential(new PositionElevator(1, true));

It's more annoying, but thems the breaks.

I'm still looking at other things. Your AutonomousInit is screwy. You're starting the ZeroElevator command in there. Twice. Commands ONLY run when Scheduler->Run() is called. It's never called in AutonInit, so that command only starts to run at the first call of AutonomousPeriodic. You've already added a CommandGroup requiring the elevator after that, so I'm pretty sure the Zero Elevator command gets immediately cancelled without even initializing.

I'm not sure about your idea for triggering commands from other commands. I've never needed to do it, so I have no idea if it will work.

My only other suggestion at the moment is to start up NetConsole, cause it gives you much more detailed debugging info and you might be able to tell what's hanging up the scheduler there.

[cbf]

Kevin --

Thanks for looking at this. Actually, pretty much all the glitches you point out we've already fixed. Sorry for the confusion about branches, but the latest is at: https://github.com/ligerbots/Recycle...tialDartmouth2.

Note that one of the orange hats at the UMass Dartmouth meet today suggested moving the AddSequentials out of the autonomous class constructors, but it made no difference. I'll probably move them back in.

There are likely more errors to be discovered -- if could only at least get our autonomous command groups to be run..

[Kevin Sevcik]

Putting kids to bed, so I can't delve deeper right now. I'll admit I was looking at things before your Friday commit so I could see what y'all were talking about. I'll try to have more thoughts in an hour or two. I'm suspicious of default commands you might be running hanging things.

[cbf]

Well here's a new data point:

All of our Autonomous Command classes had a #include "../robot.h" in them (the students can be pretty sloppy about these things). I had meant to clean this up, but you know.. more important things like actually making the program work got in the way.

Simply out of suspicion, I decided to clean these up.

Now our "do nothing" autonomous command is successfully executed, but unfortunately the ones that actually do something still never return from
Scheduler::GetInstance()->Run();

I'm guessing that there was some C++ class, possibly even name confusion.

At least this is now looking less hopeless and somewhat debuggable. The tree has been updated with this.