SOFT564Z Group 3 / Mbed 2 deprecated SOFT564Z_Group_3v46666

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Servo ros_lib_kinetic

main.cpp@2:6197e1cf2cd1, 2019-10-24 (annotated)

Committer:
Stumi
Date:
Thu Oct 24 14:24:27 2019 +0000
Revision:
2:6197e1cf2cd1
Parent:
1:9bc7f95c3c7d
Child:
3:df6160e2f6d9
Motor code successfully integrated

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