File content as of revision 0:9e0195f256b1:

#include "mbed.h"
#include "XNucleo53L0A1.h"
#include <stdio.h>

Serial pc(USBTX,USBRX);

// Setup the shutdown pins for the VL53L0Xs
// When these pins are pulled LOW, the sensor goes into shutdown.

DigitalOut center_shutx(p26);

DigitalOut left_shdn(p18);

DigitalOut center_shdn(p19);
DigitalOut right_shdn(p20);


// This VL53L0X board test application performs a range measurement in polling mode
// Use 3.3(Vout) for Vin, p28 for SDA, p27 for SCL, P26 for shdn on mbed LPC1768

//I2C sensor pins
#define VL53L0_I2C_SDA   p28
#define VL53L0_I2C_SCL   p27

#define MY_DEBUG 0

static XNucleo53L0A1 *board=NULL;

// Orange = left 
// Blue = right
// Yellow = forward
// Green = backwards

DigitalOut TurnLeft(p21);
DigitalOut TurnRight(p22);
//
DigitalOut Forward(p23);
DigitalOut Reverse(p24);

void InitChannel(DevI2C *device_i2c, int WhichChannel)
{
    int status;
    
    if(board != NULL)
    {
        // Call destructor
        board->~XNucleo53L0A1();
        board = NULL;
    }
    
    /* creates the 53L0A1 expansion board singleton obj */
    board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2);
    
    left_shdn = 0;
    right_shdn = 0;
    center_shdn = 0;
    
    switch(WhichChannel)
    {
        // left channel
        case 0:    
        
        left_shdn = 1;
        
        break;
        
        // right channel
        case 2:
  
        right_shdn = 1;
        
        break;
        
        // center channel
        case 1:
        default:
        
        center_shdn = 1;
        
        break;
    }
    
    center_shutx = 0; //must reset sensor for an mbed reset to work
    wait(0.001);
    center_shutx = 1;
    //wait(0.01);
    
    /* init the 53L0A1 board with default values */
    status = board->init_board();
    while (status) {
        
        if(MY_DEBUG) {
        pc.printf("Failed to init board! \r\n");
        }
        
        status = board->init_board();
    }
}

void TurnLeft()
{
    TurnLeft = 1;
    wait(0.1);
    TurnLeft = 0;
}

void TurnRight()
{
    TurnLeft = 0;
    TurnRight = 1;
    wait(0.1);
    TurnRight = 0;   
}


void Drive()
{
        
}

void Reverse()
{
    
}






uint32_t left_distance;
uint32_t left_distance_prev;

uint32_t center_distance;
uint32_t center_distance_prev;

uint32_t right_distance;
uint32_t right_distance_prev;

// Flashes all 4 leds with right channel selected after n minutes.
// 12:31-12:32 start
// 


int main()
{
    int status;
    uint32_t distance;
    
    TurnLeft = 0;
    TurnRight = 0;
    Forward = 0;
    Reverse = 0;
    
    left_distance = 2500;
    left_distance_prev = 2500;

    center_distance = 2500;
    center_distance_prev = 2500;

    right_distance = 2500;
    right_distance_prev = 2500;
    
    int state = 0;
    
    DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL);
    
    device_i2c->frequency(3000000);
    
    // Init the center channel
    InitChannel(device_i2c, 1);
    
    
    //loop taking and printing distance
   // bool didSwitchOver = false;
    int i = 0;
    bool CountUp = true;
    
    uint32_t TheTest = 0;
    
    while (1) {
        
        status = board->sensor_centre->get_distance(&distance);
        
        if (status == VL53L0X_ERROR_NONE) {
            pc.printf("D=%ld mm\r\n", distance);
            
            
            switch(state)
            {
                // Right
                case 2:
                    
                    TheTest = right_distance_prev;
                    right_distance_prev = right_distance;
                    right_distance = distance;
                    
                    // Handle step functions (hopefully) gracefully.
                    if(TheTest <= 500 && distance > 500)
                    {
                        TurnRight = 0;
                        
                    }
                    else
                    {
                        TheTest = (uint32_t)((right_distance + right_distance_prev)/2);
                        
                        if(  TheTest <= 500 )
                        {
                            TurnRight = 1;
                        }
                        else
                        {
                            TurnRight = 0;
                        }
                        
                    }
                
                break;
                
                // Left
                case 0:
                    TheTest = left_distance_prev;
                    left_distance_prev = left_distance;
                    left_distance = distance;
                    
                    // Handle step functions (hopefully) gracefully.
                    if(TheTest <= 500 && distance > 500)
                    {
                        TurnLeft = 0;    
                    }
                    else
                    {
                        TheTest = (uint32_t)((left_distance + left_distance_prev)/2);
                        
                        
                        
                        if( TheTest <= 500 )
                        {
                            TurnLeft = 1;
                        }
                        else
                        {
                            TurnLeft = 0;
                        }
                    
                    }
                
                break;
                
                // Center
                case 1:
                default:
                    
                    TheTest = center_distance_prev;
                    
                    center_distance_prev = center_distance;
                    center_distance = distance;
                    
                    // Handle step functions (hopefully) gracefully. 
                    if(TheTest <= 500 && distance > 500 )
                    {
                        Forward = 0;
                        Reverse = 0;
                    }
                    else
                    {
                        
                        TheTest = (uint32_t)((center_distance + center_distance_prev)/2);
                        
                        if( TheTest <= 200) 
                        {
                               Forward = 1;
                               Reverse = 0;
                        }
                        else if( TheTest >= 200 && TheTest <= 500 )
                        {
                            Forward = 0;
                            Reverse = 1;
                        }
                        else
                        {
                            Forward = 0;
                            Reverse = 0;
                        }
                    
                    }
                    
                    
                
                break;   
            }
            
            
            
            
            
            i++;
        }
        else
        {
            if(MY_DEBUG) {
            pc.printf("COuld not get distance.\r\n");
            }
            distance = 2500; // 2500 mm    
        }
        
        // Take 5 measurements on the channel
        if( i > 2) {
                
                if(CountUp)
                {
                
                    if( state == 2 )
                    {
                        state--;
                        CountUp = false; 
                    }
                    else
                    {
                        state++;
                        CountUp = true;   
                    }
                    
                }
                else
                {
                    if( state == 0 )
                    {
                        state++;
                        CountUp = true;   
                    }
                    else
                    {
                        state--;
                        CountUp = false;   
                    }
                }
                
                if(MY_DEBUG) {
                    pc.printf("Doing switch over\r\n");
                }
                InitChannel(device_i2c, state);
                
                if(MY_DEBUG) {   
                    pc.printf("Finishing switch over\r\n");
                }
                
                i = 0;
            }
    }
}