Diff: main.cpp

Revision:

0:59ae1ba97a94

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Sep 28 02:07:41 2017 +0000
@@ -0,0 +1,144 @@
+#include "mbed.h"
+#include "Sabertooth.h"
+#include "rtos.h"
+#include "peixingv2.h"
+#include "LidarLitev2.h"
+
+Serial pc(USBTX, USBRX);
+Serial device(p28, p27);
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+DigitalIn SignalIn(p29);
+DigitalOut SignalOut(p30);
+LidarLitev2 Lidar(p9, p10);
+PwmOut servo(p21);
+
+AnalogIn s1 (p15);
+AnalogIn s2 (p16);
+AnalogIn s3 (p19);
+AnalogIn s4 (p20);
+
+#define k_5 12466.0
+#define k_4 -23216.0
+#define k_3 14974.0
+#define k_2 -3585.0
+#define k_1 19.0
+#define k_0 96.0
+
+int i;
+float val1, dist1, total_dist1, ave_dist1;
+float val2, dist2, total_dist2, ave_dist2;
+bool x;
+
+void read_sensors()
+{
+    dist1 = 0;
+    total_dist1 = 0;
+    ave_dist1 = 0;
+
+    dist2 = 0;
+    total_dist2 = 0;
+    ave_dist2 = 0;
+
+    for(i = 0; i < 50; i++) 
+    {
+        val1 = s1.read();
+        dist1 = 0;
+        dist1 += k_5*(val1*val1*val1*val1*val1);
+        dist1 += k_4*(val1*val1*val1*val1);
+        dist1 += k_3*(val1*val1*val1);
+        dist1 += k_2*(val1*val1);
+        dist1 += k_1*val1;
+        dist1 += k_0;
+        total_dist1 = total_dist1 + dist1;
+
+        val2 = s2.read();
+        dist2 = 0;
+        dist2 += k_5*(val2*val2*val2*val2*val2);
+        dist2 += k_4*(val2*val2*val2*val2);
+        dist2 += k_3*(val2*val2*val2);
+        dist2 += k_2*(val2*val2);
+        dist2 += k_1*val2;
+        dist2 += k_0;
+        total_dist2 = total_dist2 + dist2;
+    }
+    ave_dist1 = total_dist1 / 50;
+    ave_dist2 = total_dist2 / 50;
+}
+
+void thread1(void const *args)
+{
+    while(true)
+    {
+        read_sensors();
+        int speed = 20;
+        if(ave_dist1 < 30 || ave_dist2 < 30) SignalOut = 1;
+        else SignalOut = 0;
+        if(ave_dist1 < 30 && ave_dist2 >= 30) 
+        {
+            x = false;
+            turnright(speed);
+        }
+        else if(ave_dist2 < 30 && ave_dist1 >= 30) 
+        {
+            x = false;
+            turnleft(speed);
+        }
+        else if(ave_dist2 < 30 && ave_dist1 < 30) 
+        {
+            x = false;
+            turnright(speed);
+        }
+        else x = true;
+    }
+}
+
+int main()
+{
+    pc.baud(115200);
+    device.baud(115200);
+    front_sb.InitializeCom();
+    back_sb.InitializeCom();
+    Thread thread(thread1);
+    int speed = 50;
+    float distance;
+    char c = 'k', a;
+    Lidar.configure();
+    servo.period_ms(20);
+    while(true) 
+    {
+        if(x == true)
+        {
+            if(device.readable())
+            {
+                a = device.getc();
+                if(a > 0 && a <= 100) speed = a;
+                else if(a == 108)
+                {
+                    distance = 0;
+                    for(int j = 0; j < 3; j++) distance = distance + Lidar.distance();
+                    distance = distance / 3;
+                    pc.printf("%d", int(distance));
+                    device.putc(int(distance));
+                }
+                else if(a >= 109)
+                {
+                    servo.pulsewidth_us(1089 + (a - 109) * 7);
+                    wait(0.01);
+                }
+                else c = a;
+            }
+            if(c == 'g') forward(speed);
+            else if(c == 'h') backward(speed);
+            else if(c == 'j') right(speed);
+            else if(c == 'i') left(speed);
+            else if(c == 'e') turnright(speed);
+            else if(c == 'f') turnleft(speed);
+            else stop();
+        }
+    }
+}
\ No newline at end of file