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

$nodeclass ParkArm : StateNode {

	$nodeclass RelaxArm : StateNode : doStart {	

		SharedObject<PIDMC> pid;
		MC_ID 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("parkArm.mot") =C=> RelaxArm

	}

}

REGISTER_BEHAVIOR(ParkArm);