 
 void loop()
 {
-    detectObject()
-
-    switch (state)
+    // --------------AFSTANDSBEDIENING UITVRAGEN---------------
+    if (!isExecutingCommand())
     {
-        case 0:
-            delay(1000);
-            state = 1;
+        switch (command)
+        {
+        case 0x00:
+            start();
             break;
-        case 1:
-            if (cmd_MoveToMat.IsFinished())
-            {
-                cmd_MoveToMat.Start();
-            }
-
-            if (cmd_MoveToMat.IsFinished())
-            {
-                state++;
-            }
-            cmd_MoveToMat.Update();
+        case 0x01:
             break;
-        case 2:
-            if (cmd_Rotate.IsFinished())
-            {
-                rotateRight();
-            }
-
-            if (cmd_Rotate.IsFinished())
-            {
-                state++;
-            }
-            break;
-        case 3:
-            if (cmd_MoveForward.IsFinished())
-            {
-                cmd_MoveForward.Start();
-            }
-            if (cmd_MoveForward.IsFinished())
-            {
-                state++;
-            }
-            break;
-        case 4:
-            if (cmd_Rotate.IsFinished())
-            {
-                rotateLeft();
-            }
-
-            if (cmd_Rotate.IsFinished())
-            {
-                state++;
-            }
-            break;
-        case 5:
-            if (cmd_Move.IsFinished())
-            {
-                cmd_Move.Start();
-            }
-
-            if (cmd_Move.IsFinished())
-            {
-                state++;
-            }
-            break;
+        }
     }
 
+    processUpdate();
     vehicle.Update();
     cmd_Dodge.Update();
     cmd_MoveForward.Update();
     cmd_Move.Update();
     cmd_Rotate.Update();
-
-}
-
-void moveForward()
-{
-    vehicle.Forward(60000);
-}
-
-void moveBackward()
-{
-    vehicle.Backward(60000);
 }
 
 void stop()

Command_MoveForward cmd_MoveForward(vehicle);
Command_Move cmd_Move(vehicle)


int state = -1;
unsigned long _time = millis();

void setup()
{

        return true;
    }

    if (!(cmd_Dodge.IsFinished() || cmd_MoveToMat.IsFinished() || cmd_Rotate.IsFinished() || cmd_Move.IsFinished() || cmd_MoveForward.IsFinished() || _processing))
    {
        return true;
    }

    return false;
}

void start()
{
    _processing = true;
    state = 1;
}

void processUpdate()
{
    switch (state)
    {
    case 1:
        if (cmd_MoveToMat.IsFinished())
        {
            delay(1000);
            cmd_MoveToMat.Start();
        }

        if (cmd_MoveToMat.IsFinished())
        {
            state++;
        }
        cmd_MoveToMat.Update();
        break;
    case 2:
        if (cmd_Rotate.IsFinished())
        {
            cmd_Rotate.Right();
        }

        if (cmd_Rotate.IsFinished())
        {
            state++;
        }
        break;
    case 3:
        if (cmd_MoveForward.IsFinished())
        {
            cmd_MoveForward.Start();
        }
        if (cmd_MoveForward.IsFinished())
        {
            state++;
        }
        break;
    case 4:
        if (cmd_Rotate.IsFinished())
        {
            cmd_Rotate.Left();
        }

        if (cmd_Rotate.IsFinished())
        {
            state++;
        }
        break;
    case 5:
        if (cmd_Move.IsFinished())
        {
            cmd_Move.Start();
        }

        if (cmd_Move.IsFinished())
        {
            state = 0;
            _processing = false;
        }
        break;
    }
}