Nan Li / Mbed 2 deprecated tof_main

A gesture project based on X_NUCLRO_53L1A1

Dependencies:   Test_SSD1306 mbed X_NUCLEO_53L1A1

function.cpp@1:183f249ff8c0, 2022-09-02 (annotated)

Committer:
nanecho
Date:
Fri Sep 02 16:04:16 2022 +0000
Revision:
1:183f249ff8c0
Parent:
0:5ea7efe9f6c9 
A gesture project based on X_NUCLEO_53L1A1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nanecho 0:5ea7efe9f6c9 1 #include <stdint.h>
nanecho 0:5ea7efe9f6c9 2 #include <stdio.h>
nanecho 0:5ea7efe9f6c9 3 #include "mbed.h"
nanecho 0:5ea7efe9f6c9 4 #include "XNucleo53L1A1.h"
nanecho 0:5ea7efe9f6c9 5 #include "VL53L1X_I2C.h"
nanecho 0:5ea7efe9f6c9 6
nanecho 0:5ea7efe9f6c9 7 extern uint16_t distance_centre; //the distance data of the central sensor
nanecho 0:5ea7efe9f6c9 8 extern uint16_t distance_left; //the distance data of the left sensor
nanecho 0:5ea7efe9f6c9 9 extern uint16_t distance_right; //the distance data of the right sensor
nanecho 0:5ea7efe9f6c9 10 extern uint8_t state;
nanecho 0:5ea7efe9f6c9 11 extern uint8_t command;
nanecho 0:5ea7efe9f6c9 12
nanecho 0:5ea7efe9f6c9 13 static XNucleo53L1A1 *board; //initialize the hardware
nanecho 0:5ea7efe9f6c9 14 DigitalOut myled(LED1); //initialize the led and named it as "myled"
nanecho 0:5ea7efe9f6c9 15 int wait_fun(); //initialize the wait_function
nanecho 0:5ea7efe9f6c9 16
nanecho 0:5ea7efe9f6c9 17 int wait_fun()
nanecho 0:5ea7efe9f6c9 18 {
nanecho 0:5ea7efe9f6c9 19 if(state == 0) // First stage
nanecho 0:5ea7efe9f6c9 20 {
nanecho 0:5ea7efe9f6c9 21 // instructive led on, device is ready
nanecho 0:5ea7efe9f6c9 22 myled = 1;
nanecho 0:5ea7efe9f6c9 23 // ‘Waiting area’
nanecho 0:5ea7efe9f6c9 24 }
nanecho 0:5ea7efe9f6c9 25 else if( distance_centre > 50 && distance_centre < 80 )
nanecho 0:5ea7efe9f6c9 26 {
nanecho 0:5ea7efe9f6c9 27 command = 3;
nanecho 0:5ea7efe9f6c9 28 state = 11; //forward to the second state
nanecho 0:5ea7efe9f6c9 29
nanecho 0:5ea7efe9f6c9 30 myled = 0; //led is off, program begins
nanecho 0:5ea7efe9f6c9 31 }
nanecho 0:5ea7efe9f6c9 32 else if( distance_centre > 400 ) // ‘Stop’
nanecho 0:5ea7efe9f6c9 33 {
nanecho 0:5ea7efe9f6c9 34 command = 9;
nanecho 0:5ea7efe9f6c9 35 state = 0; //Back to the first stage
nanecho 0:5ea7efe9f6c9 36 }
nanecho 0:5ea7efe9f6c9 37
nanecho 0:5ea7efe9f6c9 38
nanecho 0:5ea7efe9f6c9 39 if(state == 11) // Second stage
nanecho 0:5ea7efe9f6c9 40 {
nanecho 0:5ea7efe9f6c9 41 // ‘Waiting area’
nanecho 0:5ea7efe9f6c9 42 }
nanecho 0:5ea7efe9f6c9 43 else if(distance_left > 1 && distance_left < 200&& distance_centre >50 && distance_centre < 80 && distance_right > 1 &&distance_right < 200)
nanecho 0:5ea7efe9f6c9 44 {
nanecho 0:5ea7efe9f6c9 45 command = 3;
nanecho 0:5ea7efe9f6c9 46 state = 11; // Back to second stage
nanecho 0:5ea7efe9f6c9 47 }
nanecho 0:5ea7efe9f6c9 48
nanecho 0:5ea7efe9f6c9 49 // ‘Turning right’
nanecho 0:5ea7efe9f6c9 50
nanecho 0:5ea7efe9f6c9 51 else if(distance_right < 80 && distance_right > 50&&distance_centre < 80 && distance_centre > 50&&distance_left>1000)
nanecho 0:5ea7efe9f6c9 52 {
nanecho 0:5ea7efe9f6c9 53 command=7;
nanecho 0:5ea7efe9f6c9 54 state = 11;
nanecho 0:5ea7efe9f6c9 55 }
nanecho 0:5ea7efe9f6c9 56 //’Turing left’
nanecho 0:5ea7efe9f6c9 57
nanecho 0:5ea7efe9f6c9 58 else if(distance_left < 80 && distance_left > 50 &&distance_centre< 80 && distance_centre > 50 &&distance_right>50&&distance_right >1000)
nanecho 0:5ea7efe9f6c9 59 {
nanecho 0:5ea7efe9f6c9 60 command=8;
nanecho 0:5ea7efe9f6c9 61 state = 11;
nanecho 0:5ea7efe9f6c9 62 }
nanecho 0:5ea7efe9f6c9 63 // ‘Going forward’
nanecho 0:5ea7efe9f6c9 64 else if(distance_right > 80 && distance_right < 400 &&distance_centre > 80 && distance_centre < 400 &&distance_left > 80&& distance_left <400)
nanecho 0:5ea7efe9f6c9 65 {
nanecho 0:5ea7efe9f6c9 66 command = 1;
nanecho 0:5ea7efe9f6c9 67 state = 11;
nanecho 0:5ea7efe9f6c9 68 }
nanecho 0:5ea7efe9f6c9 69 // ‘Going left forward’
nanecho 0:5ea7efe9f6c9 70 else if (distance_left < 400 && distance_left > 80 &&distance_centre< 400 && distance_centre > 80 &&distance_right>1000)
nanecho 0:5ea7efe9f6c9 71 {
nanecho 0:5ea7efe9f6c9 72 command = 5;
nanecho 0:5ea7efe9f6c9 73 state =11 ;
nanecho 0:5ea7efe9f6c9 74 }
nanecho 0:5ea7efe9f6c9 75
nanecho 0:5ea7efe9f6c9 76 // ‘Going right forward’
nanecho 0:5ea7efe9f6c9 77 else if (distance_right < 400 && distance_right > 8&&distance_centre < 400 && distance_centre > 80&&distance_left>1000)
nanecho 0:5ea7efe9f6c9 78 {
nanecho 0:5ea7efe9f6c9 79 command =6;
nanecho 0:5ea7efe9f6c9 80 state =11 ;
nanecho 0:5ea7efe9f6c9 81 }
nanecho 0:5ea7efe9f6c9 82 // ‘Going back’
nanecho 0:5ea7efe9f6c9 83 else if(distance_left < 50 && distance_left >1 && distance_centre <50 && distance_centre >1 && distance_right < 50 &&distance_right >1 )
nanecho 0:5ea7efe9f6c9 84 {
nanecho 0:5ea7efe9f6c9 85 command=2;
nanecho 0:5ea7efe9f6c9 86 state = 11;
nanecho 0:5ea7efe9f6c9 87 }
nanecho 0:5ea7efe9f6c9 88 // ‘Going left backward’
nanecho 0:5ea7efe9f6c9 89 else if (distance_left < 50 && distance_left > 1 &&distance_centre< 50 && distance_centre > 1 &&distance_right>1000)
nanecho 0:5ea7efe9f6c9 90 {
nanecho 0:5ea7efe9f6c9 91 command =55;
nanecho 0:5ea7efe9f6c9 92 state =0 ;
nanecho 0:5ea7efe9f6c9 93 }
nanecho 0:5ea7efe9f6c9 94 // ‘Going right backward’
nanecho 0:5ea7efe9f6c9 95 else if (distance_right < 50 && distance_right > 1 &&distance_centre< 50 && distance_centre > 1 &&distance_left>1000)
nanecho 0:5ea7efe9f6c9 96 {
nanecho 0:5ea7efe9f6c9 97 command =66;
nanecho 0:5ea7efe9f6c9 98 state =0 ;
nanecho 0:5ea7efe9f6c9 99 }
nanecho 0:5ea7efe9f6c9 100 // ‘Stop’
nanecho 0:5ea7efe9f6c9 101 else if( distance_centre > 400 )
nanecho 0:5ea7efe9f6c9 102 {
nanecho 0:5ea7efe9f6c9 103 command = 9;
nanecho 0:5ea7efe9f6c9 104 state = 0;
nanecho 0:5ea7efe9f6c9 105 }
nanecho 0:5ea7efe9f6c9 106
nanecho 0:5ea7efe9f6c9 107 return 0;
nanecho 0:5ea7efe9f6c9 108 }