#include "Behaviors/StateMachine.h"

$nodeclass TwoReach : StateNode {

	$nodeclass Reach1 : PostureNode : doStart {
		getMC()->setMaxSpeed(ArmBaseOffset, 1);
		getMC()->setMaxSpeed(ArmShoulderOffset, 1);
		getMC()->setMaxSpeed(ArmWristOffset, 1);
		getMC()->setMaxSpeed(WristRotateOffset, 1);
		cout << getMC()->getMaxSpeed(ArmBaseOffset) << endl;
		this->loadFile("reach1.pos");
	}

	$nodeclass Reach2 : PostureNode : doStart {
		getMC()->setMaxSpeed(ArmBaseOffset, 1);
		getMC()->setMaxSpeed(ArmShoulderOffset, 1);
		getMC()->setMaxSpeed(ArmWristOffset, 1);
		getMC()->setMaxSpeed(WristRotateOffset, 1);
		cout << getMC()->getMaxSpeed(ArmBaseOffset) << endl;
		this->loadFile("reach2.pos");
	}

	$setupmachine {
		startnode: Reach1 =T(3000)=> Reach2
	}

}
REGISTER_BEHAVIOR(TwoReach);