#include "Behaviors/StateMachine.h"
#include "Behaviors/Controller.h"
#include "Shared/RobotInfo.h"
#include "Motion/MMAccessor.h"
#include "Motion/ArmMC.h"
#include "Shared/RobotInfo.h"
#include "Motion/PIDMC.h"

$nodeclass Lab6Q6 : StateNode{
	$nodeclass Relax : StateNode : doStart{
		MotionManager::MC_ID pidMCID;	
		SharedObject<PIDMC> pid;
		pidMCID = motman->addPersistentMotion(pid,MotionManager::kHighPriority);
		MMAccessor<PIDMC> relaxer(pidMCID);
		for (unsigned int joint = 0; joint < NumArmJoints; joint++)
			relaxer->setJointPowerLevel(ArmOffset + joint, 0);
		processEvent(EventBase(EventBase::timerEGID,1,EventBase::statusETID,0));
		//erouter->addTimer(this,0,500);
	}
	$setupmachine {
		DynamicMotionSequenceNode[setFile("dock.mot");] =C=> Relax
	}
}

REGISTER_BEHAVIOR(Lab6Q6);