Diff: main.cpp

Revision:

1:e47cc3d6d036

Parent:

0:41c798d23e8f

--- a/main.cpp	Thu Apr 26 22:26:51 2018 +0000
+++ b/main.cpp	Thu Apr 26 22:29:28 2018 +0000
@@ -1,12 +1,142 @@
 #include "mbed.h"
+#include "Servo.h"
+#include "rtos.h"
+#include "ultrasonic.h"
+
+/// Objects
+DigitalIn pb(p22);
+Serial blue(p28,p27); // Bluetooh
+Servo myservo(p21); // Motor
+Serial pc(USBTX,USBRX); // PC
+Thread t1;
+Thread t2;
+Thread t3;
+
+Mutex off;
+bool close=false;
 
-DigitalOut myled(LED1);
+void bluetooth()
+{
+    char bnum = 0;
+    myservo=0.5;
+    Thread::wait(2000);
+    while(1) {
+
+        Thread::wait(50);
+        if (blue.getc() == '!') {
+            if (blue.getc()=='B') {
+                bnum = blue.getc();
+                pc.printf("%s",bnum);
+                // Up Arrow
+
+                if ((bnum == '5') && (myservo <= 1.0)) {
+                    myservo = myservo + 0.05;
+                    pc.printf("\n Servo up\n ");
+
+                }
 
-int main() {
-    while(1) {
-        myled = 1;
-        wait(0.2);
-        myled = 0;
-        wait(0.2);
+                // Down Arrow
+                else if ((bnum == '6') && (myservo >= 0.54)) {
+                    myservo = myservo - 0.05;
+                    pc.printf("\n Servo down\n ");
+                }
+
+                // Left Arrow
+
+                else if (bnum == '7') {
+                    pc.printf("\n brake \n ");
+
+                    while (myservo > 0.0) {
+                         if (myservo < 0.05)
+                             myservo = 0.0;
+                         else
+                             myservo = myservo - 0.005; // Brake
+                              pc.printf("brekae");
+                     }  
+
+                }
+
+                // Right Arrow
+
+                else if (bnum == '8') {
+                    //servo.lock();
+                    myservo = 0.5; // Neutral
+                    pc.printf("\n neutral");
+
+
+                }
+            }
+        }
+    }
+
+
+}// ends bluetooh
+
+
+
+void dist(int distance)
+{
+    //put code here to execute when the distance has changed
+    pc.printf("Distance %d mm\r\n", distance);
+    if(distance<400) {
+        close=true;
+
     }
 }
+
+ultrasonic mu(p6, p7, .1, 1, &dist);
+
+void stop()
+{
+
+
+ while (myservo > 0.0) {
+                         if (myservo < 0.05)
+                             myservo = 0.0;
+                         else
+                             myservo = myservo - 0.008; // Brake
+                              pc.printf("brake");
+                     }  
+
+
+
+
+
+}//ends stop
+
+int main()
+{
+    pb.mode(PullUp);
+    pc.printf("Program Begins ");
+    myservo = 0.0;
+    wait(0.5); //detects signal
+//Required ESC Calibration/Arming sequence
+//sends longest and shortest PWM pulse to learn and arm at power on
+    myservo = 1.0; //send longest PWM
+    wait(8);
+    myservo = 0.0; //send shortest PWM
+    wait(8);
+//ESC now operational using standard servo PWM signals
+    pc.printf("Before");
+
+
+    pc.printf("\n SETUP \n ");
+
+    t1.start(bluetooth);
+
+    mu.startUpdates();
+
+    while (true) {
+
+        if(close) {
+            pc.printf("\n Thread released \n ");
+            t2.start(stop);
+            Thread::wait(1000);
+            close=false;
+        }
+
+        mu.checkDistance();
+
+    }//ends While
+
+}// ends Main
\ No newline at end of file