with the tof code

Dependencies:   mbed Servo ros_lib_kinetic

main.cpp

Committer:
Stumi
Date:
2019-10-22
Revision:
0:6021ec1b1449
Child:
1:9bc7f95c3c7d

File content as of revision 0:6021ec1b1449:

/*--------------------------------------------------------------------------------
Filename: main.cpp
Description: Main program loop
--------------------------------------------------------------------------------*/
#include "mbed.h"
#include "TOF.h"

Serial pc(SERIAL_TX, SERIAL_RX, 9600);    // set-up serial to pc
cAdafruit_VL6180X VL6180X;                  //Define TOF sensor class

DigitalOut CS1(PC_8);
DigitalOut CS2(PC_9);

int main()
{
    int range1, range2;
    
    CS1 = 0;
    CS2 = 0;
    wait(0.01);

    CS1 = 1;
    wait(0.01);
    VL6180X.Setup(DEFAULT_ADDRESS, ADDRESS1);
    VL6180X.Init(ADDRESS1);
    VL6180X.Start_Range(ADDRESS1);
    
    CS2 = 1;
    wait(0.01);
    VL6180X.Setup(DEFAULT_ADDRESS, ADDRESS2);
    VL6180X.Init(ADDRESS2);
    VL6180X.Start_Range(ADDRESS2);
    
    
    
    pc.printf("INITIALISED\n\r");

    while(1) {
        
        // poll the VL6180X till new sample ready
        VL6180X.Poll_Range(ADDRESS1);
        pc.printf("POLL1");

        // read range result
        range1 = VL6180X.Read_Range(ADDRESS1);
        pc.printf("READ RANGE1");
        
        // clear the interrupt on VL6180X
        VL6180X.Clear_Interrupts(ADDRESS1);
        pc.printf("READRANGE1");
        
        // poll the VL6180X till new sample ready
        VL6180X.Poll_Range(ADDRESS2);
        pc.printf("POLL2");

        // read range result
        range2 = VL6180X.Read_Range(ADDRESS2);
        pc.printf("READ RANGE2");
        
        // clear the interrupt on VL6180X
        VL6180X.Clear_Interrupts(ADDRESS2);
        pc.printf("READRANGE2");
        
        // send range to pc by serial
        pc.printf("SENSOR1:%d SENSOR2: %d\r\n", range1, range2);
        wait(0.1);
    }
}