float point[3], dist; 

float myState[12];


void init(){

     point[0] = 0.0;
     point[1] =-0.1;
     point[2] = 0.0; 

}

void loop(){
 
    api.getMyZRState(myState);

    dist = distanza(point);
    
    if (dist > 0.015)

	api.setPositionTarget(point);
 
    else
        DEBUG(("dist= %5.3f posiz= %5.3f %5.3f %5.3f ",
               	dist, myState[0], myState[1], myState[2]));
}


float distanza (float *target) {

    float distanceSquared = (target[0] - myState[0]) * (target[0] - myState[0])
                          + (target[1] - myState[1]) * (target[1] - myState[1])
                          + (target[2] - myState[2]) * (target[2] - myState[2]);

    return sqrtf(distanceSquared);
}