#include "Behaviors/Demos/Navigation/PilotDemo.h"

$nodeclass Circumnavigate : PilotDemo {

    $setupmachine{
         startdemo: TurnCCW(2.0943951*1.2) =C=> GoForward(1000*1.2) =C=> TurnCW(1.57079633*1.2) =C=> GoForward(866*1.2) =C=> TurnCW(1.57079633*1.2) =C=> GoForward(1650*1.1) =C=> TurnCW(1.04719755*1.2) =C=> GoForward(1649*1.2) =C=> TurnCW(1.57079633*1.2) =C=> GoForward(866*1.2) =C=> TurnCW(1.57079633*1.2) =C=> GoForward(1000*1.4) =C=> TurnCW(1.04719755*1.3) =C=> PostMachineCompletion
    }

    $nodeclass TurnCCW(float x) : PilotNode(PilotTypes::walk) : doStart {
         pilotreq.da = x;
    }

    $nodeclass TurnCW(float x) : PilotNode(PilotTypes::walk) : doStart {
         pilotreq.da = -x;
    }

    $nodeclass GoForward(float x) : PilotNode(PilotTypes::walk) : doStart {
        pilotreq.dx = x; // forward half a meter
    }

}

REGISTER_BEHAVIOR(Circumnavigate);