File content as of revision 1:183f249ff8c0:

#include <stdint.h>
#include <stdio.h>
#include "mbed.h"
#include "XNucleo53L1A1.h"
#include "VL53L1X_I2C.h"

extern uint16_t distance_centre; //the distance data of the central sensor
extern uint16_t distance_left; //the distance data of the left sensor
extern uint16_t distance_right; //the distance data of the right sensor
extern uint8_t state;
extern uint8_t command;

static XNucleo53L1A1 *board; //initialize the hardware
DigitalOut myled(LED1); //initialize the led and named it as "myled"
int wait_fun(); //initialize the wait_function

int wait_fun()
{
    if(state == 0) // First stage
    {
        // instructive led on, device is ready
        myled = 1;
        // ‘Waiting area’
    }
    else if( distance_centre > 50 && distance_centre < 80 )
    {
        command = 3;
        state = 11; //forward to the second state
                
        myled = 0; //led is off, program begins
    }
    else if( distance_centre > 400 ) // ‘Stop’
    {
        command = 9;
        state = 0; //Back to the first stage
    }
    
    
    if(state == 11) // Second stage
    {
        // ‘Waiting area’
    }
        else if(distance_left > 1 && distance_left < 200&& distance_centre >50 && distance_centre < 80 && distance_right > 1 &&distance_right < 200)
        {
            command = 3;
            state = 11; // Back to second stage
        }
                    
        // ‘Turning right’
                
        else if(distance_right < 80 && distance_right > 50&&distance_centre < 80 && distance_centre > 50&&distance_left>1000)
        {
            command=7;
            state = 11;
        }
        //’Turing left’
                
        else if(distance_left < 80 && distance_left > 50 &&distance_centre< 80 && distance_centre > 50 &&distance_right>50&&distance_right >1000)
        {
            command=8;
            state = 11;
        }
        // ‘Going forward’
        else if(distance_right > 80 && distance_right < 400 &&distance_centre > 80 && distance_centre < 400 &&distance_left > 80&& distance_left <400)
        {
            command = 1;
            state = 11;
        }
        // ‘Going left forward’
        else if (distance_left < 400 && distance_left > 80 &&distance_centre< 400 && distance_centre > 80 &&distance_right>1000)
        {
            command = 5;
            state =11 ;
        }
           
        // ‘Going right forward’
        else if (distance_right < 400 && distance_right > 8&&distance_centre < 400 && distance_centre > 80&&distance_left>1000)
        {
            command =6;
            state =11 ;
        }
        // ‘Going back’
        else if(distance_left < 50 && distance_left >1 && distance_centre <50 && distance_centre >1 && distance_right < 50 &&distance_right >1 )
        {
            command=2;
            state = 11;
        }
        // ‘Going left backward’
        else if (distance_left < 50 && distance_left > 1 &&distance_centre< 50 && distance_centre > 1 &&distance_right>1000)
        {
            command =55;
            state =0 ;
        }
        // ‘Going right backward’
        else if (distance_right < 50 && distance_right > 1 &&distance_centre< 50 && distance_centre > 1 &&distance_left>1000)
        {
            command =66;
            state =0 ;
        }
        // ‘Stop’
        else if( distance_centre > 400 )
        {
            command = 9;
            state = 0;
        }
    
    return 0;
}