//-*-c++-*-
#ifndef _MtestD_h_
#define _MtestD_h_

#include "Behaviors/StateMachine.h"
#include "Motion/DynamicMotionSequence.h"
#include "Motion/MotionManager.h"

class MtestD : public DynamicMotionSequenceNode {
public:
	MtestD() : DynamicMotionSequenceNode("Mtest") {}

	virtual void DoStart() {
		MMAccessor<DynamicMotionSequence> dm(getMC_ID());
		dm->clear();
		dm->advanceTime(50);
		dm->setOutputCmd(RobotInfo::HeadOffset, 0.0);
		dm->setOutputCmd(RobotInfo::HeadOffset+1, 0.0);
		dm->advanceTime(50);
		dm->setOutputCmd(RobotInfo::HeadOffset, 0.2);
		dm->setOutputCmd(RobotInfo::HeadOffset+1, 0.2);
		dm->advanceTime(50);
		dm->setOutputCmd(RobotInfo::HeadOffset, 0.4);
		dm->setOutputCmd(RobotInfo::HeadOffset+1, 0.4);
	}

};

#endif