SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v3

with the tof code

Dependencies:   mbed Servo ros_lib_kinetic

main.cpp@4:36a04230554d, 2019-11-07 (annotated)

Committer:
Stumi
Date:
Thu Nov 07 15:31:52 2019 +0000
Revision:
4:36a04230554d
Parent:
3:df6160e2f6d9
Child:
5:bc5081f0c063
Added power management class and obstacle avoidance

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 4:36a04230554d 7 #include "Motor.h"
Stumi 4:36a04230554d 8 #include "power.h"
Stumi 0:6021ec1b1449 9
Stumi 0:6021ec1b1449 10 Serial pc(SERIAL_TX, SERIAL_RX, 9600); // set-up serial to pc
Stumi 1:9bc7f95c3c7d 11
Stumi 1:9bc7f95c3c7d 12 //TOF chip shutdown signals
Stumi 4:36a04230554d 13 DigitalOut TOF1(PC_8);
Stumi 4:36a04230554d 14 DigitalOut TOF4(PC_11);
Stumi 4:36a04230554d 15 DigitalOut TOF6(PC_12);
Stumi 4:36a04230554d 16 DigitalOut TOF7(PD_2);
Stumi 0:6021ec1b1449 17
Stumi 4:36a04230554d 18 //Class define
Stumi 4:36a04230554d 19 cAdafruit_VL6180X VL6180X(TOF1,TOF4,TOF6,TOF7); //Define TOF sensor class and initialise devices
Stumi 4:36a04230554d 20 cPower cPower(VBATT, V5, V3);
Stumi 0:6021ec1b1449 21
Stumi 0:6021ec1b1449 22 int main()
Stumi 0:6021ec1b1449 23 {
Stumi 4:36a04230554d 24 uint8_t TOFRange[4]; //Array to store TOF measurements 0-sensor1, 1-sensor4, 2-sensor6, 3-sensor7
Stumi 4:36a04230554d 25 float power_levels[3]; //Array to voltage levels
Stumi 0:6021ec1b1449 26
Stumi 0:6021ec1b1449 27 while(1) {
Stumi 0:6021ec1b1449 28
Stumi 1:9bc7f95c3c7d 29 //Perform TOF measurements
Stumi 4:36a04230554d 30 TOFRange[0] = serviceTOF(VL6180X, ADDR1);
Stumi 4:36a04230554d 31 TOFRange[1] = serviceTOF(VL6180X, ADDR4);
Stumi 4:36a04230554d 32 TOFRange[2] = serviceTOF(VL6180X, ADDR6);
Stumi 4:36a04230554d 33 TOFRange[3] = serviceTOF(VL6180X, ADDR7);
Stumi 4:36a04230554d 34
Stumi 4:36a04230554d 35 Check_for_obstacles(TOFRange);
Stumi 4:36a04230554d 36
Stumi 1:9bc7f95c3c7d 37 //Send range to pc by serial
Stumi 4:36a04230554d 38 //pc.printf("TOF1:%d TOF3: %d TOF5: %d TOF7: %d\r\n", TOFRange[0], TOFRange[1], TOFRange[2],TOFRange[3]);
Stumi 0:6021ec1b1449 39
Stumi 4:36a04230554d 40 power_levels[0] = cPower.monitor_battery();
Stumi 4:36a04230554d 41 power_levels[1] = cPower.monitor_5v_line();
Stumi 4:36a04230554d 42 power_levels[2] = cPower.monitor_3v3_line();
Stumi 4:36a04230554d 43 printf("BATT:%4.2f, 5V:%4.2f, 3v3:%4.2f\n\r", power_levels[0], power_levels[1], power_levels[2]);
Stumi 4:36a04230554d 44
Stumi 0:6021ec1b1449 45 }
Stumi 0:6021ec1b1449 46 }