Unable to get Wait to work

Technical C/C++
1

So here’s the problem, we’re trying to make a command in where the robot drive forward for a short time, and then stops. Mainly just for testing purposes, but we ran into an issue.

Depending on which wait command we use, the robot displays different behaviour, none of which is intended. If frc2::cmd::Wait(); is used then the robot never stops waiting and continues to drive. If frc2::WaitCommand(); is used then the robot instantly skips to being finished, and does not move.

Its probably something simple which I’m just missing, any help would be greatly appreciated, all relevant files I can think of are below.

TestAuto.cpp :

#include "commands/TestAuto.h"

TestAuto::TestAuto(units::second_t time, double speed, DriveSubsystem* subsystem)
	: m_drive(subsystem), m_time(time), m_speed(speed) {
	AddRequirements({subsystem});
}

void TestAuto::Initialize() {
	m_drive->ResetEncoders();
	m_drive->ArcadeDrive(m_speed, 0.5);

// This Wait command causes the robot to never stop moving
    frc2::cmd::Wait(m_time);

//This Wait command, just doesn't work, and gets skipped over instantly
    frc2::WaitCommand(m_time);

	m_isFinished = true;
}

void TestAuto::Execute() { m_drive->ArcadeDrive(m_speed, 0); }

void TestAuto::End(bool interrupted) { m_drive->ArcadeDrive(0, 0); }

bool TestAuto::IsFinished() { return m_isFinished; }

TestAuto.h :

#pragma once

#include <frc2/command/CommandBase.h>
#include <frc2/command/CommandHelper.h>
#include <frc2/command/Commands.h>
#include <frc2/command/WaitCommand.h>
#include <frc2/command/CommandPtr.h>


#include "subsystems/DriveSubsystem.h"

class TestAuto : public frc2::CommandHelper<frc2::CommandBase, BalanceAuto> {
   public:
	/**
	 * Creates a new TestAuto.
	 *
	 * @param time The amount of seconds that the robot will drive.
	 * @param speed The speed at which the robot will drive
	 * @param drive The drive subsystem on which this command will run
	 */
	explicit TestAuto (units::second_t time, double speed, DriveSubsystem* subsystem);

	void Initialize() override;

	void Execute() override;

	void End(bool interrupted) override;

	bool IsFinished() override;

   private:
	DriveSubsystem* m_drive;
	units::second_t m_time;
	double m_speed;
	bool m_isFinished = false;
	bool m_isComposed = true;
};

And here’s the relevant RobotContainer file that runs the actual command:

#include "RobotContainer.h"

RobotContainer::RobotContainer() { ConfigureBindings(); }

void RobotContainer::ConfigureBindings() {
	m_drive.SetDefaultCommand(
		frc2::cmd::Run([this] { m_drive.ArcadeDrive(-m_driverController.GetLeftY(), m_driverController.GetRightX()); },
					   {&m_drive}));
}

frc2::CommandPtr RobotContainer::GetAutonomousCommand() {

	TestAuto testAuto = TestAuto(0.3_s, 0.5, &m_drive);
    //The print is mainly for logging reasons, but it does confirm that testAuto is being run,
    //at least we think it is.
	return frc2::CommandPtr(testAuto).AsProxy().AndThen(frc2::cmd::Print("ahoy-hoy"));
};
2

Make your TestAuto class a SequentialCommandGroup instead. Have it call a “Drive” command (whatever you want to call it) that tells your subsystem to start driving in whatever direction you need, followed by a WaitCommand, followed by a “Stop” command (could be same as the Drive command if you have it accept a speed parameter and just use zero in this case)

3

Commands don’t do anything if they’re just constructed. They need to be scheduled. If you want to wait in between steps of a command, you need a sequential composition (I recommend reading the docs on command compositions).

As for WaitCommand vs cmd::Wait, one is a command class while the latter constructs a CommandPtr holding a command of said type. I recommend using CommandPtrs, it makes objec lifetimes and ownership more obvious.

4

Thanks, I forgot to copy down an .asProxy().andThen( to the code in the post, but I will try your recommendations regarding sequential composition, and look into the command scheduler.

5

What you should do, which has worked for me, is to use the isFinished() portion of a command.

Simply get the current time when you initialize, then, in isFinished(), do a “return currentTime - startTime > delay”

Your code sets isFinished = true right away, and never stops the motors after. So it will set the motor speed to 50% but then never set it back to 0% to stop moving

So, in initialize, all you should do is set your motor speed. In End, you need to set your motor speed to zero (which you are doing). In isFinished, check the current time against the start time

In end
I

6

Okay, this is about as simple as the code can in theory get, the command is scheduled, I have confirmed. However, this code causes the robot not to stop at all. It’s not really essential anymore because we’ve switched to using encoders, but I would like to figure this out.

	return frc2::cmd::Sequence(frc2::cmd::Run([this] { this->m_drive.ArcadeDrive(0.5, 0); }),
							   frc2::cmd::Wait(0.5_s).AsProxy(),
							   frc2::cmd::Run([this] { this->m_drive.ArcadeDrive(0.0, 0); }));
};
7

Run never ends; you’re thinking of RunOnce.
Also, there’s no reason to proxy a wait.

8

I think you want the .withTimeout decorator.

If you have a driveForward command you could do…

driveForward.withTimeout(0.2);

Which would drive forward for 0.2 seconds.

(I used the Java Syntax… sorry. in C++ it is WithTimeout(0.2))

9

That still wouldn’t work, bc the motors would never be stopped.
.FinallyDo might be useful to stop the motors in case of both interrup and finish (the sequential composition option would only stop them on end).

10

In the initialize turn the drive motors on… in the end turn them off…

Maybe I don’t understand…

11

If driveCommand stops the motors in end, yeah, that would work.

12

Programming mentor for 2550 here — both of these don’t work:

	return frc2::cmd::Sequence(frc2::cmd::RunOnce([this] { this->m_drive.ArcadeDrive(0.5, 0); }).WithTimeout(0.5_s),
							   frc2::cmd::RunOnce([this] { this->m_drive.ArcadeDrive(0.0, 0); }));

	return frc2::cmd::Sequence(frc2::cmd::RunOnce([this] { this->m_drive.ArcadeDrive(0.5, 0); }),
							   frc2::cmd::Wait(0.5_s),
							   frc2::cmd::RunOnce([this] { this->m_drive.ArcadeDrive(0.0, 0); }));

Also, isn’t the whole point of Command-based that you’re able to compose commands exactly like this? I’m not sure I get the point if you’re just doing a bunch of effectively timer-based actions. Waiting and doing a thing seems like the kind of thing that should just work and be simple (hell, the second example @GarinHamburg gave is literally the one from the WPI docs).

13

Define “doesn’t work”. What’s the exhibited behavior?

14

According to them:
The first example they gave causes the motors to click, while the second example does nothing.

15

Oh, I see what might be the problem: you’re not declaring any requirements. That means that there’s a fair chance that the motor controllers are getting set from multiple places, that could cause that clicking.
Declare all requirements, don’t proxy anything (there’s no reason, and it might be causing commands to interrupt each other), and let’s see what code causes what behavior.

16

I don’t think you should be using RunOnce. Motors require continues values to run, mostly for safety causes.

17
18

The robot already drives, the issue here is really an issue of not being able to use wait in a command structure. Based on the docs it should be as simple as

frc::cmd::Wait(1.0_s).AsProxy().AndThen(m_drive->ArcadeDrive(0, 0));
19

Also, given the contradictions here (and the fact that something this basic is this obtuse at all), those in this thread who work on WPILib have some real work to do on API ergonomics. It is possible to write something humans can use, but it takes work, and you actually have to document things well.

20

Why are you using a proxy?