Diff: main.cpp

Revision:

0:7d7387eefc5a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 02 04:26:40 2018 +0000
@@ -0,0 +1,149 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "Servo.h"
+#include "ultrasonic.h"
+
+Motor m1(p25, p15, p14); // pwm, fwd, rev
+Motor m2(p26, p19, p20 ); // pwm, fwd, rev
+volatile bool move = 1; 
+Serial bluemod(p9,p10);
+Serial pc(USBTX,USBRX);
+Servo myservo(p24);
+DigitalOut myled1(LED1); 
+DigitalOut myled2(LED2); 
+DigitalOut myled3(LED3); 
+DigitalOut myled4(LED4); 
+
+
+ 
+//C union can convert 4 chars to a float - puts them in same location in memory
+//trick to pack the 4 bytes from Bluetooth serial port back into a 32-bit float
+union f_or_char {
+    float f;
+    char  c[4];
+};
+
+void dist(int distance)
+{
+    if (distance < 500.0) {
+    m1.speed(0.0);
+    m2.speed(0.0);
+    wait(1);
+    myservo = 0.0;
+    wait(1);
+    m1.speed(0.3);
+    m2.speed(-0.3);
+    wait(1.5);
+    m1.speed(0.0);
+    m2.speed(0.0);
+    wait(1.5); 
+    move = 0;
+    myservo = 1.0;
+    myled1 = 1;
+    myled2 = 1;
+    myled3 = 1;
+    myled4 = 1; 
+    wait(1);
+    myservo = 0.0;
+    myled1 = 0;
+    myled2 = 0;
+    myled3 = 0;
+    myled4 = 0; 
+    wait(1);
+    }
+}
+
+ultrasonic sonar(p6, p7, .1, 1, &dist);
+ 
+int main()
+{
+    float s1;
+    float s2;
+    char bchecksum=0;
+    char temp=0;
+    union f_or_char x,y,z;
+    
+    sonar.startUpdates();//start measuring the distance
+    
+    while(1) {
+        bchecksum=0;
+        if (bluemod.getc()=='!') {
+            if (bluemod.getc()=='A') { //Accelerometer data packet
+                for (int i=0; i<4; i++) {
+                    temp = bluemod.getc();
+                    x.c[i] = temp;
+                    bchecksum = bchecksum + temp;
+                }
+                for (int i=0; i<4; i++) {
+                    temp = bluemod.getc();
+                    y.c[i] = temp;
+                    bchecksum = bchecksum + temp;
+                }
+                for (int i=0; i<4; i++) {
+                    temp = bluemod.getc();
+                    z.c[i] = temp;
+                    bchecksum = bchecksum + temp;
+                }
+                if (bluemod.getc()==char(~('!' + 'A' + bchecksum))) { //checksum OK?
+                //pc.printf("X = %f  Y = %f  Z = %f\n\r",x.f, y.f, z.f);         
+                    move = 1;
+                    if (x.f < -4.0) {
+                        s1 = 0.3;
+                        s2 = -0.3;
+                        m1.speed(s1);
+                        m2.speed(s2);
+                        myled1 = 1;
+                        myled2 = 0;
+                        myled3 = 0;
+                        myled4 = 0; 
+                        wait(0.05);
+                        }
+                    if (x.f > 4.0) {
+                        s1 = -0.3;
+                        s2 = 0.3;
+                        m1.speed(s1);
+                        m2.speed(s2);
+                        myled1 = 0;
+                        myled2 = 1;
+                        myled3 = 0;
+                        myled4 = 0; 
+                        wait(0.05);
+                        }
+                    if (y.f < -3.5) {
+                        s1 = -0.2;
+                        s2 = -0.2;
+                        m1.speed(s1);
+                        m2.speed(s2);
+                        myled1 = 0;
+                        myled2 = 0;
+                        myled3 = 1;
+                        myled4 = 0; 
+                        wait(0.05);
+                        }
+                    if (y.f > 3.0) {
+                        s1 = 0.2;
+                        s2 = 0.2;
+                        m1.speed(s1);
+                        m2.speed(s2);
+                        myled1 = 0;
+                        myled2 = 0;
+                        myled3 = 0;
+                        myled4 = 1; 
+                        wait(0.05);
+                        }     
+                    if (x.f > -4.0 && x.f < 4.0 && y.f > -4.0 && y.f <4.0) {
+                        s1 = 0.0;
+                        s2 = 0.0;
+                        m1.speed(s1);
+                        m2.speed(s2);
+                        wait(0.05); 
+                        }  
+                    while(move) {
+                        sonar.checkDistance();
+                        move = 0;
+                    }        
+                }
+            }
+        }
+    }
+}