eljer chua / Mbed 2 deprecated Open_House_Mbed1_Actual

aaa

Dependencies:   LidarLitev2 Sabertoothaaa mbed-rtos mbed

main.cpp@0:59ae1ba97a94, 2017-09-28 (annotated)

Committer:
eljerchua
Date:
Thu Sep 28 02:07:41 2017 +0000
Revision:
0:59ae1ba97a94
aaa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eljerchua 0:59ae1ba97a94 1 #include "mbed.h"
eljerchua 0:59ae1ba97a94 2 #include "Sabertooth.h"
eljerchua 0:59ae1ba97a94 3 #include "rtos.h"
eljerchua 0:59ae1ba97a94 4 #include "peixingv2.h"
eljerchua 0:59ae1ba97a94 5 #include "LidarLitev2.h"
eljerchua 0:59ae1ba97a94 6
eljerchua 0:59ae1ba97a94 7 Serial pc(USBTX, USBRX);
eljerchua 0:59ae1ba97a94 8 Serial device(p28, p27);
eljerchua 0:59ae1ba97a94 9
eljerchua 0:59ae1ba97a94 10 DigitalOut led1(LED1);
eljerchua 0:59ae1ba97a94 11 DigitalOut led2(LED2);
eljerchua 0:59ae1ba97a94 12 DigitalOut led3(LED3);
eljerchua 0:59ae1ba97a94 13 DigitalOut led4(LED4);
eljerchua 0:59ae1ba97a94 14
eljerchua 0:59ae1ba97a94 15 DigitalIn SignalIn(p29);
eljerchua 0:59ae1ba97a94 16 DigitalOut SignalOut(p30);
eljerchua 0:59ae1ba97a94 17 LidarLitev2 Lidar(p9, p10);
eljerchua 0:59ae1ba97a94 18 PwmOut servo(p21);
eljerchua 0:59ae1ba97a94 19
eljerchua 0:59ae1ba97a94 20 AnalogIn s1 (p15);
eljerchua 0:59ae1ba97a94 21 AnalogIn s2 (p16);
eljerchua 0:59ae1ba97a94 22 AnalogIn s3 (p19);
eljerchua 0:59ae1ba97a94 23 AnalogIn s4 (p20);
eljerchua 0:59ae1ba97a94 24
eljerchua 0:59ae1ba97a94 25 #define k_5 12466.0
eljerchua 0:59ae1ba97a94 26 #define k_4 -23216.0
eljerchua 0:59ae1ba97a94 27 #define k_3 14974.0
eljerchua 0:59ae1ba97a94 28 #define k_2 -3585.0
eljerchua 0:59ae1ba97a94 29 #define k_1 19.0
eljerchua 0:59ae1ba97a94 30 #define k_0 96.0
eljerchua 0:59ae1ba97a94 31
eljerchua 0:59ae1ba97a94 32 int i;
eljerchua 0:59ae1ba97a94 33 float val1, dist1, total_dist1, ave_dist1;
eljerchua 0:59ae1ba97a94 34 float val2, dist2, total_dist2, ave_dist2;
eljerchua 0:59ae1ba97a94 35 bool x;
eljerchua 0:59ae1ba97a94 36
eljerchua 0:59ae1ba97a94 37 void read_sensors()
eljerchua 0:59ae1ba97a94 38 {
eljerchua 0:59ae1ba97a94 39 dist1 = 0;
eljerchua 0:59ae1ba97a94 40 total_dist1 = 0;
eljerchua 0:59ae1ba97a94 41 ave_dist1 = 0;
eljerchua 0:59ae1ba97a94 42
eljerchua 0:59ae1ba97a94 43 dist2 = 0;
eljerchua 0:59ae1ba97a94 44 total_dist2 = 0;
eljerchua 0:59ae1ba97a94 45 ave_dist2 = 0;
eljerchua 0:59ae1ba97a94 46
eljerchua 0:59ae1ba97a94 47 for(i = 0; i < 50; i++)
eljerchua 0:59ae1ba97a94 48 {
eljerchua 0:59ae1ba97a94 49 val1 = s1.read();
eljerchua 0:59ae1ba97a94 50 dist1 = 0;
eljerchua 0:59ae1ba97a94 51 dist1 += k_5*(val1*val1*val1*val1*val1);
eljerchua 0:59ae1ba97a94 52 dist1 += k_4*(val1*val1*val1*val1);
eljerchua 0:59ae1ba97a94 53 dist1 += k_3*(val1*val1*val1);
eljerchua 0:59ae1ba97a94 54 dist1 += k_2*(val1*val1);
eljerchua 0:59ae1ba97a94 55 dist1 += k_1*val1;
eljerchua 0:59ae1ba97a94 56 dist1 += k_0;
eljerchua 0:59ae1ba97a94 57 total_dist1 = total_dist1 + dist1;
eljerchua 0:59ae1ba97a94 58
eljerchua 0:59ae1ba97a94 59 val2 = s2.read();
eljerchua 0:59ae1ba97a94 60 dist2 = 0;
eljerchua 0:59ae1ba97a94 61 dist2 += k_5*(val2*val2*val2*val2*val2);
eljerchua 0:59ae1ba97a94 62 dist2 += k_4*(val2*val2*val2*val2);
eljerchua 0:59ae1ba97a94 63 dist2 += k_3*(val2*val2*val2);
eljerchua 0:59ae1ba97a94 64 dist2 += k_2*(val2*val2);
eljerchua 0:59ae1ba97a94 65 dist2 += k_1*val2;
eljerchua 0:59ae1ba97a94 66 dist2 += k_0;
eljerchua 0:59ae1ba97a94 67 total_dist2 = total_dist2 + dist2;
eljerchua 0:59ae1ba97a94 68 }
eljerchua 0:59ae1ba97a94 69 ave_dist1 = total_dist1 / 50;
eljerchua 0:59ae1ba97a94 70 ave_dist2 = total_dist2 / 50;
eljerchua 0:59ae1ba97a94 71 }
eljerchua 0:59ae1ba97a94 72
eljerchua 0:59ae1ba97a94 73 void thread1(void const *args)
eljerchua 0:59ae1ba97a94 74 {
eljerchua 0:59ae1ba97a94 75 while(true)
eljerchua 0:59ae1ba97a94 76 {
eljerchua 0:59ae1ba97a94 77 read_sensors();
eljerchua 0:59ae1ba97a94 78 int speed = 20;
eljerchua 0:59ae1ba97a94 79 if(ave_dist1 < 30 || ave_dist2 < 30) SignalOut = 1;
eljerchua 0:59ae1ba97a94 80 else SignalOut = 0;
eljerchua 0:59ae1ba97a94 81 if(ave_dist1 < 30 && ave_dist2 >= 30)
eljerchua 0:59ae1ba97a94 82 {
eljerchua 0:59ae1ba97a94 83 x = false;
eljerchua 0:59ae1ba97a94 84 turnright(speed);
eljerchua 0:59ae1ba97a94 85 }
eljerchua 0:59ae1ba97a94 86 else if(ave_dist2 < 30 && ave_dist1 >= 30)
eljerchua 0:59ae1ba97a94 87 {
eljerchua 0:59ae1ba97a94 88 x = false;
eljerchua 0:59ae1ba97a94 89 turnleft(speed);
eljerchua 0:59ae1ba97a94 90 }
eljerchua 0:59ae1ba97a94 91 else if(ave_dist2 < 30 && ave_dist1 < 30)
eljerchua 0:59ae1ba97a94 92 {
eljerchua 0:59ae1ba97a94 93 x = false;
eljerchua 0:59ae1ba97a94 94 turnright(speed);
eljerchua 0:59ae1ba97a94 95 }
eljerchua 0:59ae1ba97a94 96 else x = true;
eljerchua 0:59ae1ba97a94 97 }
eljerchua 0:59ae1ba97a94 98 }
eljerchua 0:59ae1ba97a94 99
eljerchua 0:59ae1ba97a94 100 int main()
eljerchua 0:59ae1ba97a94 101 {
eljerchua 0:59ae1ba97a94 102 pc.baud(115200);
eljerchua 0:59ae1ba97a94 103 device.baud(115200);
eljerchua 0:59ae1ba97a94 104 front_sb.InitializeCom();
eljerchua 0:59ae1ba97a94 105 back_sb.InitializeCom();
eljerchua 0:59ae1ba97a94 106 Thread thread(thread1);
eljerchua 0:59ae1ba97a94 107 int speed = 50;
eljerchua 0:59ae1ba97a94 108 float distance;
eljerchua 0:59ae1ba97a94 109 char c = 'k', a;
eljerchua 0:59ae1ba97a94 110 Lidar.configure();
eljerchua 0:59ae1ba97a94 111 servo.period_ms(20);
eljerchua 0:59ae1ba97a94 112 while(true)
eljerchua 0:59ae1ba97a94 113 {
eljerchua 0:59ae1ba97a94 114 if(x == true)
eljerchua 0:59ae1ba97a94 115 {
eljerchua 0:59ae1ba97a94 116 if(device.readable())
eljerchua 0:59ae1ba97a94 117 {
eljerchua 0:59ae1ba97a94 118 a = device.getc();
eljerchua 0:59ae1ba97a94 119 if(a > 0 && a <= 100) speed = a;
eljerchua 0:59ae1ba97a94 120 else if(a == 108)
eljerchua 0:59ae1ba97a94 121 {
eljerchua 0:59ae1ba97a94 122 distance = 0;
eljerchua 0:59ae1ba97a94 123 for(int j = 0; j < 3; j++) distance = distance + Lidar.distance();
eljerchua 0:59ae1ba97a94 124 distance = distance / 3;
eljerchua 0:59ae1ba97a94 125 pc.printf("%d", int(distance));
eljerchua 0:59ae1ba97a94 126 device.putc(int(distance));
eljerchua 0:59ae1ba97a94 127 }
eljerchua 0:59ae1ba97a94 128 else if(a >= 109)
eljerchua 0:59ae1ba97a94 129 {
eljerchua 0:59ae1ba97a94 130 servo.pulsewidth_us(1089 + (a - 109) * 7);
eljerchua 0:59ae1ba97a94 131 wait(0.01);
eljerchua 0:59ae1ba97a94 132 }
eljerchua 0:59ae1ba97a94 133 else c = a;
eljerchua 0:59ae1ba97a94 134 }
eljerchua 0:59ae1ba97a94 135 if(c == 'g') forward(speed);
eljerchua 0:59ae1ba97a94 136 else if(c == 'h') backward(speed);
eljerchua 0:59ae1ba97a94 137 else if(c == 'j') right(speed);
eljerchua 0:59ae1ba97a94 138 else if(c == 'i') left(speed);
eljerchua 0:59ae1ba97a94 139 else if(c == 'e') turnright(speed);
eljerchua 0:59ae1ba97a94 140 else if(c == 'f') turnleft(speed);
eljerchua 0:59ae1ba97a94 141 else stop();
eljerchua 0:59ae1ba97a94 142 }
eljerchua 0:59ae1ba97a94 143 }
eljerchua 0:59ae1ba97a94 144 }