SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v3

with the tof code

Dependencies:   mbed Servo ros_lib_kinetic

main.cpp@1:9bc7f95c3c7d, 2019-10-24 (annotated)

Committer:
Stumi
Date:
Thu Oct 24 13:35:54 2019 +0000
Revision:
1:9bc7f95c3c7d
Parent:
0:6021ec1b1449
Child:
2:6197e1cf2cd1
All four TOF sensors operational and easily serviceable

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Stumi 0:6021ec1b1449 1 /*--------------------------------------------------------------------------------
Stumi 0:6021ec1b1449 2 Filename: main.cpp
Stumi 0:6021ec1b1449 3 Description: Main program loop
Stumi 0:6021ec1b1449 4 --------------------------------------------------------------------------------*/
Stumi 0:6021ec1b1449 5 #include "mbed.h"
Stumi 0:6021ec1b1449 6 #include "TOF.h"
Stumi 0:6021ec1b1449 7
Stumi 0:6021ec1b1449 8 Serial pc(SERIAL_TX, SERIAL_RX, 9600); // set-up serial to pc
Stumi 1:9bc7f95c3c7d 9
Stumi 1:9bc7f95c3c7d 10 //TOF chip shutdown signals
Stumi 1:9bc7f95c3c7d 11 DigitalOut TOF1(PC_8);
Stumi 1:9bc7f95c3c7d 12 DigitalOut TOF3(PC_10);
Stumi 1:9bc7f95c3c7d 13 DigitalOut TOF5(PC_12);
Stumi 1:9bc7f95c3c7d 14 DigitalOut TOF7(PG_2);
Stumi 1:9bc7f95c3c7d 15
Stumi 1:9bc7f95c3c7d 16 cAdafruit_VL6180X VL6180X(TOF1,TOF3,TOF5,TOF7); //Define TOF sensor class and initialise devices
Stumi 0:6021ec1b1449 17
Stumi 1:9bc7f95c3c7d 18 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 19 Function name: ServiceTOF
Stumi 1:9bc7f95c3c7d 20 Input Parameters: address - address of target TOF sensor
Stumi 1:9bc7f95c3c7d 21 Output Parameters: range - distance measurement in mm
Stumi 1:9bc7f95c3c7d 22 Description: performs measurement routine on a given sensor
Stumi 1:9bc7f95c3c7d 23 ----------------------------------------------------------------------------------*/
Stumi 1:9bc7f95c3c7d 24 uint8_t serviceTOF(uint8_t address){
Stumi 1:9bc7f95c3c7d 25
Stumi 1:9bc7f95c3c7d 26 uint8_t range = 0;
Stumi 1:9bc7f95c3c7d 27
Stumi 1:9bc7f95c3c7d 28 // poll the VL6180X till new sample ready
Stumi 1:9bc7f95c3c7d 29 VL6180X.Poll_Range(address);
Stumi 0:6021ec1b1449 30
Stumi 1:9bc7f95c3c7d 31 // read range result
Stumi 1:9bc7f95c3c7d 32 range = VL6180X.Read_Range(address);
Stumi 1:9bc7f95c3c7d 33
Stumi 1:9bc7f95c3c7d 34 // clear the interrupt on VL6180X
Stumi 1:9bc7f95c3c7d 35 VL6180X.Clear_Interrupts(address);
Stumi 1:9bc7f95c3c7d 36
Stumi 1:9bc7f95c3c7d 37 return range;
Stumi 1:9bc7f95c3c7d 38 }
Stumi 1:9bc7f95c3c7d 39
Stumi 1:9bc7f95c3c7d 40
Stumi 1:9bc7f95c3c7d 41 /*--------------------------------------------------------------------------------
Stumi 1:9bc7f95c3c7d 42 Function name: main
Stumi 1:9bc7f95c3c7d 43 Input Parameters: N/A
Stumi 1:9bc7f95c3c7d 44 Output Parameters: N/A
Stumi 1:9bc7f95c3c7d 45 Description: Main programming loop
Stumi 1:9bc7f95c3c7d 46 ----------------------------------------------------------------------------------*/
Stumi 0:6021ec1b1449 47 int main()
Stumi 0:6021ec1b1449 48 {
Stumi 1:9bc7f95c3c7d 49 uint8_t TOFRange[4]; //Array to store TOF measurements 0-sensor1, 1-sensor3, 2-sensor5, 3-sensor7
Stumi 0:6021ec1b1449 50
Stumi 0:6021ec1b1449 51 while(1) {
Stumi 0:6021ec1b1449 52
Stumi 1:9bc7f95c3c7d 53 //Perform TOF measurements
Stumi 1:9bc7f95c3c7d 54 TOFRange[0] = serviceTOF(ADDR1);
Stumi 1:9bc7f95c3c7d 55 TOFRange[1] = serviceTOF(ADDR2);
Stumi 1:9bc7f95c3c7d 56 TOFRange[2] = serviceTOF(ADDR3);
Stumi 1:9bc7f95c3c7d 57 TOFRange[3] = serviceTOF(ADDR4);
Stumi 1:9bc7f95c3c7d 58
Stumi 1:9bc7f95c3c7d 59 //Send range to pc by serial
Stumi 1:9bc7f95c3c7d 60 pc.printf("TOF1:%d TOF3: %d TOF5: %d TOF7: %d\r\n", TOFRange[0], TOFRange[1], TOFRange[2],TOFRange[3]);
Stumi 0:6021ec1b1449 61
Stumi 1:9bc7f95c3c7d 62 //Short delay
Stumi 0:6021ec1b1449 63 wait(0.1);
Stumi 0:6021ec1b1449 64 }
Stumi 0:6021ec1b1449 65 }