[MrRoboSteve]

This is what Ryan from 3081 put together. We did a bit of testing of it at Wisconsin, will be bashing it more in the next few days.

// header

class SwapCommandCommand: public CommandBase {
private:
	bool swapState;
	Command *commandOne;
	Command *commandTwo;
	Command *runningCommand;
public:
	SwapCommandCommand(Command *commandOne, Command *commandTwo);
	virtual void Initialize();
	virtual void Execute();
	virtual bool IsFinished();
	virtual void End();
	virtual void Interrupted();
	void Reset();
};


// cpp
SwapCommandCommand::SwapCommandCommand(Command *commandOne, Command *commandTwo) {
	// Use requires() here to declare subsystem dependencies
	// eg. requires(chassis);
	this->commandOne = commandOne;
	this->commandTwo = commandTwo;
	//False for command one
	//True for command Two
	this->swapState = false;
	this->runningCommand = NULL;
}

void SwapCommandCommand::Initialize() {
	if(swapState) {
		printf("[SwapCommandCommand] Scheduling Command two\n");
		this->runningCommand = commandTwo;
	} else {
		this->runningCommand = commandOne;
		printf("[SwapCommandCommand] Scheduling Command One\n");
	}
	this->runningCommand->Start();
}

void SwapCommandCommand::Execute() {
}


bool SwapCommandCommand::IsFinished() {
	return !runningCommand->IsRunning();
}

void SwapCommandCommand::End() {
	printf("[SwapCommandCommand] The command has finished\n");
	swapState = !swapState;
}

void SwapCommandCommand::Interrupted() {
	printf("[SwapCommandCommand] Interrupted canceling command\n");
	this->runningCommand->Cancel();
}


void SwapCommandCommand::Reset() {
	this->swapState = false;
}