zadanie 1 roboty mobilne

#include "Aria.h"
#include <stdio.h>
/* Główna Metoda */
void fwd(int mm, int s, void *fun());
void rot(int deg, int s, void *fun());
void ifnotmoved();
int main(int argc, char **argv)
{
    /* Robot I jego urządzenia */
    Aria::init(); //Zainicjowanie biblioteki ARIA
    ArRobot robot; //Deklaracja głównej klasy roboczej ArRobot
    ArArgumentParser parser(&argc, argv); //Deklaracja parsera
    parser.loadDefaultArguments(); //Ładuje domyślne ustawienia parsera
    ArRobotConnector robotConnector(&parser, &robot);
    ArLaserConnector laserConnector(&Parser, &robot, &robotConnector);
    Parser.addDefaultArgument("-connectLaser");
    //Deklaracja połączenia z serverem (mikrokonrolerem)
    /* Połączenie z robotem */
    if(Parser.checkHelpAndWarnUnparsed())
    {
        cout << "Unknown settings\n"; //Jeśli błąd parsera, zamknij program
        Aria::exit(1);
    }

    if (!robotConnector.connectRobot(&robot))
    {
        cout << "Unable to connect\n"; //Jeśli brak połączenia, zamknij
        Aria::exit(1);
    }
    robot.runAsync(true); //Uruchomienie w trybie asynchronicznym

    fwd(1100,200,ifnotmoved());
    rot(-30,10,ifnotmoved());
    fwd(1000,100,ifnotmoved());
    rot(-80,15,ifnotmoved());
    fwd(1600,250,ifnotmoved());
    rot(15,20,ifnotmoved());
    fwd(600,300,ifnotmoved());


    Aria::exit(0); //Zakończenie programu
}
void fwd(int mm, int s, void (*fun)()){
    robot.setTransVelMax(s);
    fun();
    robot.lock();
    fun();
    robot.move(mm); // Przemieszczenie o 1100mm
    fun();
    while(!robot.isMoveDone()){
            fun();
    }
    robot.unlock();
}
void rot(int deg, int s, void (*fun)()){
    robot.setRotVelMax(s);
    fun();
    robot.lock();
    fun();
    robot.setDeltaHeading(deg);
    fun();
    while(!robot.isHeadingDone(0))){
        fun();
    }
    robot.unlock();
}
void ifnotmoved(){
    int i=0;
    while(robot.checkRangeDevicesCumulativePolar(-10, +10)<600){
        Ar::sleep(1000);
        i++;

    }
    if(i==10)Aria::exit(0);
}