Diff: main.cpp

Revision:

5:57196959be02

Parent:

4:137b119e5198

--- a/main.cpp	Tue May 11 18:35:32 2021 +0000
+++ b/main.cpp	Sun May 08 20:12:11 2022 +0000
@@ -1,80 +1,71 @@
 #include "mbed.h"
-#include "hcsr04.h"
+#include "HCSR04.h"
+#include "tb6612.h"
+//Library for controlling ultrasonic module HCSR04
+//Ported by hiawoood from arduino library orgininally created by ITead studio.
+HCSR04 sensorF(D7,D2); //Create sensor object of type HCSR04 (class borrowed HCSR04.cpp), with pins D2 and D3 assigned to trig and echo respectively
+HCSR04 sensorL(D12,D13);
+//HCSR04 sensorR(D9,D10);
+TB6612 motorL(D9,D10,D11);
+TB6612 motorR(D3,D4,D5);
+Thread turnL;
+Thread turnR;
+Thread turnN;
 
-HCSR04 hcsr04(D2,D3); //Create hcsr04 object of type HCSR04 (class made in hcsr04.cpp), with pins D2 and D3 assigned to trig and echo respectively
-DigitalOut beep(D4); //using code made for motor modified to be just a pwm object, create beep of this type and assign D4 to it, this can then be used to control beep speed
-DigitalIn gButton(BUTTON1);
+void turnLFN() {
+    motorL.setSpeed(-1);
+    motorR.setSpeed(1);
+    thread_sleep_for(1500);
+    }
+    
+void turnRFN() {
+    motorR.setSpeed(-1);
+    motorL.setSpeed(1);
+    thread_sleep_for(1500);
+    }
 
+void turnNFN() {
+    motorL.setSpeed(1);
+    motorR.setSpeed(1);
+    }
+
+//long distanceL = sensorL.distance(CM);
+int turn = 5000;
 
 int main()
 {
+    long distanceF;
+    long distanceL;
+
     while(true) {
+        distanceF = sensorF.distance(CM);
+        distanceL = sensorL.distance(CM);
 
-        if (gButton == 0) {
-            hcsr04.distance();
-            while (gButton == 1) {
+        if (distanceL<10 && distanceF>10) {
+            /*motorL.setSpeed(0.50);
+            motorR.setSpeed(0.50);
+            printf ("forward \r\n");*/
+            turnN.start(turnNFN);
+        }
 
-                if (hcsr04.distance() <= 15) { //retrive the distance value and compare against a condition, depending on condition it meets print a message and set the beep speed
-                    printf("Uncomfortably Close \r\n");
-                    beep = true;
-                    thread_sleep_for(100);
-                } else {
-                    if (hcsr04.distance() <= 30) {
-                        printf("Very Close \r\n");
-                        beep = true ;
-                        thread_sleep_for(100);
-                        beep = false;
-                        thread_sleep_for(100);
-                    } else {
-                        if (hcsr04.distance() <= 60) {
-                            printf("Close \r\n");
-                            beep = true;
-                            thread_sleep_for(100);
-                            beep = false;
-                            thread_sleep_for(200);
-                        } else {
-                            if (hcsr04.distance() <= 100) {
-                                printf("Safe \r\n");
-                                beep = true;
-                                thread_sleep_for(100);
-                                beep = false;
-                                thread_sleep_for(400);
-                            } else {
-                                if (hcsr04.distance() <= 200) {
-                                    printf("Far \r\n");
-                                    beep = true;
-                                    thread_sleep_for(100);
-                                    beep = false;
-                                    thread_sleep_for(700);
-                                } else {
-                                    if (hcsr04.distance() <= 300) {
-                                        printf("Very Far \r\n");
-                                        beep = true;
-                                        thread_sleep_for(100);
-                                        beep = false;
-                                        thread_sleep_for(1200);
-                                    } else {
-                                        if (hcsr04.distance() <= 400) {
-                                            printf("Distant \r\n");
-                                            beep = false;
-                                        } else {
-                                            if (hcsr04.distance() > 400) {
-                                                printf("Out of Range \r\n");
-                                                beep = false;
-                                            }
-                                        }
-                                    }
-                                }
-                            }
-                        }
-                    }
-                }
-            }
-        } else {
-            beep = false;
+        if (distanceL>=10) {
+            /*motorL.setSpeed(-0.50);
+            motorR.setSpeed(0.50);
+            printf ("turn left \r\n");
+            thread_sleep_for(turn);
+            
+            motorL.setSpeed(0.50);
+            motorR.setSpeed(0.50);
+            thread_sleep_for(300);*/
+            turnL.start(turnLFN);
         }
-        //thread_sleep_for(500); //put the thread to sleep for 500ms so the distance isn't read uncontrollably fast.
+
+        if (distanceL<10 && distanceF<10) {
+            /*motorL.setSpeed(0.50);
+            motorR.setSpeed(-0.50);
+            printf("turn right \r\n");
+            thread_sleep_for(turn);*/
+            turnR.start(turnRFN);
+        }
     }
-}
-
-//AAAAAAAAAAAAAAAAAa
+}
\ No newline at end of file