Diff: function.cpp

Revision:

0:5ea7efe9f6c9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/function.cpp	Fri Sep 02 16:02:17 2022 +0000
@@ -0,0 +1,108 @@
+#include <stdint.h>
+#include <stdio.h>
+#include "mbed.h"
+#include "XNucleo53L1A1.h"
+#include "VL53L1X_I2C.h"
+
+extern uint16_t distance_centre; //the distance data of the central sensor
+extern uint16_t distance_left; //the distance data of the left sensor
+extern uint16_t distance_right; //the distance data of the right sensor
+extern uint8_t state;
+extern uint8_t command;
+
+static XNucleo53L1A1 *board; //initialize the hardware
+DigitalOut myled(LED1); //initialize the led and named it as "myled"
+int wait_fun(); //initialize the wait_function
+
+int wait_fun()
+{
+    if(state == 0) // First stage
+    {
+        // instructive led on, device is ready
+        myled = 1;
+        // ‘Waiting area’
+    }
+    else if( distance_centre > 50 && distance_centre < 80 )
+    {
+        command = 3;
+        state = 11; //forward to the second state
+                
+        myled = 0; //led is off, program begins
+    }
+    else if( distance_centre > 400 ) // ‘Stop’
+    {
+        command = 9;
+        state = 0; //Back to the first stage
+    }
+    
+    
+    if(state == 11) // Second stage
+    {
+        // ‘Waiting area’
+    }
+        else if(distance_left > 1 && distance_left < 200&& distance_centre >50 && distance_centre < 80 && distance_right > 1 &&distance_right < 200)
+        {
+            command = 3;
+            state = 11; // Back to second stage
+        }
+                    
+        // ‘Turning right’
+                
+        else if(distance_right < 80 && distance_right > 50&&distance_centre < 80 && distance_centre > 50&&distance_left>1000)
+        {
+            command=7;
+            state = 11;
+        }
+        //’Turing left’
+                
+        else if(distance_left < 80 && distance_left > 50 &&distance_centre< 80 && distance_centre > 50 &&distance_right>50&&distance_right >1000)
+        {
+            command=8;
+            state = 11;
+        }
+        // ‘Going forward’
+        else if(distance_right > 80 && distance_right < 400 &&distance_centre > 80 && distance_centre < 400 &&distance_left > 80&& distance_left <400)
+        {
+            command = 1;
+            state = 11;
+        }
+        // ‘Going left forward’
+        else if (distance_left < 400 && distance_left > 80 &&distance_centre< 400 && distance_centre > 80 &&distance_right>1000)
+        {
+            command = 5;
+            state =11 ;
+        }
+           
+        // ‘Going right forward’
+        else if (distance_right < 400 && distance_right > 8&&distance_centre < 400 && distance_centre > 80&&distance_left>1000)
+        {
+            command =6;
+            state =11 ;
+        }
+        // ‘Going back’
+        else if(distance_left < 50 && distance_left >1 && distance_centre <50 && distance_centre >1 && distance_right < 50 &&distance_right >1 )
+        {
+            command=2;
+            state = 11;
+        }
+        // ‘Going left backward’
+        else if (distance_left < 50 && distance_left > 1 &&distance_centre< 50 && distance_centre > 1 &&distance_right>1000)
+        {
+            command =55;
+            state =0 ;
+        }
+        // ‘Going right backward’
+        else if (distance_right < 50 && distance_right > 1 &&distance_centre< 50 && distance_centre > 1 &&distance_left>1000)
+        {
+            command =66;
+            state =0 ;
+        }
+        // ‘Stop’
+        else if( distance_centre > 400 )
+        {
+            command = 9;
+            state = 0;
+        }
+    
+    return 0;
+}
\ No newline at end of file