main.cpp

00001 #include "mbed.h"
00002 #include "Sabertooth.h"
00003 #include "rtos.h"
00004 #include "peixingv2.h"
00005 #include "LidarLitev2.h"
00006 
00007 Serial pc(USBTX, USBRX);
00008 Serial device(p28, p27);
00009 
00010 DigitalOut led1(LED1);
00011 DigitalOut led2(LED2);
00012 DigitalOut led3(LED3);
00013 DigitalOut led4(LED4);
00014 
00015 DigitalIn SignalIn(p29);
00016 DigitalOut SignalOut(p30);
00017 LidarLitev2 Lidar(p9, p10);
00018 PwmOut servo(p21);
00019 
00020 AnalogIn s1 (p15);
00021 AnalogIn s2 (p16);
00022 AnalogIn s3 (p19);
00023 AnalogIn s4 (p20);
00024 
00025 #define k_5 12466.0
00026 #define k_4 -23216.0
00027 #define k_3 14974.0
00028 #define k_2 -3585.0
00029 #define k_1 19.0
00030 #define k_0 96.0
00031 
00032 int i;
00033 float val1, dist1, total_dist1, ave_dist1;
00034 float val2, dist2, total_dist2, ave_dist2;
00035 bool x;
00036 
00037 void read_sensors()
00038 {
00039     dist1 = 0;
00040     total_dist1 = 0;
00041     ave_dist1 = 0;
00042 
00043     dist2 = 0;
00044     total_dist2 = 0;
00045     ave_dist2 = 0;
00046 
00047     for(i = 0; i < 50; i++) 
00048     {
00049         val1 = s1.read();
00050         dist1 = 0;
00051         dist1 += k_5*(val1*val1*val1*val1*val1);
00052         dist1 += k_4*(val1*val1*val1*val1);
00053         dist1 += k_3*(val1*val1*val1);
00054         dist1 += k_2*(val1*val1);
00055         dist1 += k_1*val1;
00056         dist1 += k_0;
00057         total_dist1 = total_dist1 + dist1;
00058 
00059         val2 = s2.read();
00060         dist2 = 0;
00061         dist2 += k_5*(val2*val2*val2*val2*val2);
00062         dist2 += k_4*(val2*val2*val2*val2);
00063         dist2 += k_3*(val2*val2*val2);
00064         dist2 += k_2*(val2*val2);
00065         dist2 += k_1*val2;
00066         dist2 += k_0;
00067         total_dist2 = total_dist2 + dist2;
00068     }
00069     ave_dist1 = total_dist1 / 50;
00070     ave_dist2 = total_dist2 / 50;
00071 }
00072 
00073 void thread1(void const *args)
00074 {
00075     while(true)
00076     {
00077         read_sensors();
00078         int speed = 20;
00079         if(ave_dist1 < 30 || ave_dist2 < 30) SignalOut = 1;
00080         else SignalOut = 0;
00081         if(ave_dist1 < 30 && ave_dist2 >= 30) 
00082         {
00083             x = false;
00084             turnright(speed);
00085         }
00086         else if(ave_dist2 < 30 && ave_dist1 >= 30) 
00087         {
00088             x = false;
00089             turnleft(speed);
00090         }
00091         else if(ave_dist2 < 30 && ave_dist1 < 30) 
00092         {
00093             x = false;
00094             turnright(speed);
00095         }
00096         else x = true;
00097     }
00098 }
00099 
00100 int main()
00101 {
00102     pc.baud(115200);
00103     device.baud(115200);
00104     front_sb.InitializeCom();
00105     back_sb.InitializeCom();
00106     Thread thread(thread1);
00107     int speed = 50;
00108     float distance;
00109     char c = 'k', a;
00110     Lidar.configure();
00111     servo.period_ms(20);
00112     while(true) 
00113     {
00114         if(x == true)
00115         {
00116             if(device.readable())
00117             {
00118                 a = device.getc();
00119                 if(a > 0 && a <= 100) speed = a;
00120                 else if(a == 108)
00121                 {
00122                     distance = 0;
00123                     for(int j = 0; j < 3; j++) distance = distance + Lidar.distance();
00124                     distance = distance / 3;
00125                     pc.printf("%d", int(distance));
00126                     device.putc(int(distance));
00127                 }
00128                 else if(a >= 109)
00129                 {
00130                     servo.pulsewidth_us(1089 + (a - 109) * 7);
00131                     wait(0.01);
00132                 }
00133                 else c = a;
00134             }
00135             if(c == 'g') forward(speed);
00136             else if(c == 'h') backward(speed);
00137             else if(c == 'j') right(speed);
00138             else if(c == 'i') left(speed);
00139             else if(c == 'e') turnright(speed);
00140             else if(c == 'f') turnleft(speed);
00141             else stop();
00142         }
00143     }
00144 }