Programme de controle du robot joueur
Dependencies: m3piBluetooth mbed
Fork of Serial_HelloWorld_Mbed by
main.cpp@1:6d8c50dcae28, 2018-05-03 (annotated)
- Committer:
- Maximousse
- Date:
- Thu May 03 13:00:03 2018 +0000
- Revision:
- 1:6d8c50dcae28
- Parent:
- 0:879aa9d0247b
BT_Control_commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Maximousse | 1:6d8c50dcae28 | 1 | |
Maximousse | 1:6d8c50dcae28 | 2 | |
Maximousse | 1:6d8c50dcae28 | 3 | |
Maximousse | 1:6d8c50dcae28 | 4 | //-------------------------BT CONTROL |
mbed_official | 0:879aa9d0247b | 5 | #include "mbed.h" |
Maximousse | 1:6d8c50dcae28 | 6 | #include "m3pi.h" |
Maximousse | 1:6d8c50dcae28 | 7 | //#include "MSCFileSystem.h" |
Maximousse | 1:6d8c50dcae28 | 8 | |
Maximousse | 1:6d8c50dcae28 | 9 | Serial pc(USBTX, USBRX); //tx, rx |
Maximousse | 1:6d8c50dcae28 | 10 | Serial bt(p28,p27); //bluetooth |
Maximousse | 1:6d8c50dcae28 | 11 | |
Maximousse | 1:6d8c50dcae28 | 12 | |
Maximousse | 1:6d8c50dcae28 | 13 | DigitalOut myled1(LED1); |
Maximousse | 1:6d8c50dcae28 | 14 | DigitalOut myled2(LED2); |
Maximousse | 1:6d8c50dcae28 | 15 | DigitalOut myled3(LED3); |
Maximousse | 1:6d8c50dcae28 | 16 | DigitalOut myled4(LED4); |
Maximousse | 1:6d8c50dcae28 | 17 | DigitalOut leds [4] = {LED1, LED2, LED3, LED4}; |
Maximousse | 1:6d8c50dcae28 | 18 | |
Maximousse | 1:6d8c50dcae28 | 19 | m3pi m3pi; |
Maximousse | 1:6d8c50dcae28 | 20 | |
Maximousse | 1:6d8c50dcae28 | 21 | //MSCFileSystem fs("fs"); |
Maximousse | 1:6d8c50dcae28 | 22 | |
Maximousse | 1:6d8c50dcae28 | 23 | BusOut myleds(LED1, LED2, LED3, LED4); |
Maximousse | 1:6d8c50dcae28 | 24 | |
Maximousse | 1:6d8c50dcae28 | 25 | const int THRESH = 300; |
Maximousse | 1:6d8c50dcae28 | 26 | |
Maximousse | 1:6d8c50dcae28 | 27 | unsigned short sensors[5], mems[5], nbCapt, memsNb; |
Maximousse | 1:6d8c50dcae28 | 28 | char tab[4]; |
Maximousse | 1:6d8c50dcae28 | 29 | float position, proportional, last_proportional, integral; |
Maximousse | 1:6d8c50dcae28 | 30 | |
Maximousse | 1:6d8c50dcae28 | 31 | unsigned char dir; |
Maximousse | 1:6d8c50dcae28 | 32 | float speed, duration, s1, s2; |
Maximousse | 1:6d8c50dcae28 | 33 | |
Maximousse | 1:6d8c50dcae28 | 34 | |
Maximousse | 1:6d8c50dcae28 | 35 | void getCapt(bool mem = false) |
Maximousse | 1:6d8c50dcae28 | 36 | { |
Maximousse | 1:6d8c50dcae28 | 37 | int v = 0; |
Maximousse | 1:6d8c50dcae28 | 38 | nbCapt = 0; |
Maximousse | 1:6d8c50dcae28 | 39 | |
Maximousse | 1:6d8c50dcae28 | 40 | m3pi.calibrated_sensors(sensors); |
Maximousse | 1:6d8c50dcae28 | 41 | |
Maximousse | 1:6d8c50dcae28 | 42 | for(int i=0 ; i<5 ;i++) |
Maximousse | 1:6d8c50dcae28 | 43 | { |
Maximousse | 1:6d8c50dcae28 | 44 | sensors[i] = (sensors[i] > THRESH ? true : false); |
Maximousse | 1:6d8c50dcae28 | 45 | if(sensors[i]) |
Maximousse | 1:6d8c50dcae28 | 46 | { |
Maximousse | 1:6d8c50dcae28 | 47 | v += 0x1<<i; |
Maximousse | 1:6d8c50dcae28 | 48 | nbCapt++; |
Maximousse | 1:6d8c50dcae28 | 49 | } |
Maximousse | 1:6d8c50dcae28 | 50 | if(mem) |
Maximousse | 1:6d8c50dcae28 | 51 | { |
Maximousse | 1:6d8c50dcae28 | 52 | mems[i] = sensors[i]; |
Maximousse | 1:6d8c50dcae28 | 53 | memsNb = nbCapt; |
Maximousse | 1:6d8c50dcae28 | 54 | } |
Maximousse | 1:6d8c50dcae28 | 55 | } |
Maximousse | 1:6d8c50dcae28 | 56 | m3pi.leds(v); |
Maximousse | 1:6d8c50dcae28 | 57 | } |
Maximousse | 1:6d8c50dcae28 | 58 | |
Maximousse | 1:6d8c50dcae28 | 59 | |
Maximousse | 1:6d8c50dcae28 | 60 | void set_motors(float mr, float ml) |
Maximousse | 1:6d8c50dcae28 | 61 | { |
Maximousse | 1:6d8c50dcae28 | 62 | m3pi.left_motor(ml); |
Maximousse | 1:6d8c50dcae28 | 63 | m3pi.right_motor(mr); |
Maximousse | 1:6d8c50dcae28 | 64 | } |
Maximousse | 1:6d8c50dcae28 | 65 | |
Maximousse | 1:6d8c50dcae28 | 66 | void suiviIntersect(float intervalTour = 0.0f) |
Maximousse | 1:6d8c50dcae28 | 67 | { |
Maximousse | 1:6d8c50dcae28 | 68 | bt.printf("startSuivi_"); |
Maximousse | 1:6d8c50dcae28 | 69 | bool done = false; |
Maximousse | 1:6d8c50dcae28 | 70 | unsigned int idx=0; |
Maximousse | 1:6d8c50dcae28 | 71 | float vl, vr, omega, l_k=0, l_k1=0, sum=0, vmax=0.50f; |
Maximousse | 1:6d8c50dcae28 | 72 | |
Maximousse | 1:6d8c50dcae28 | 73 | #if 1 // Low speed |
Maximousse | 1:6d8c50dcae28 | 74 | const float Ki=0.005f, Kp=0.5f, Kd=1.5f, v=0.2; |
Maximousse | 1:6d8c50dcae28 | 75 | #else |
Maximousse | 1:6d8c50dcae28 | 76 | const float Ki=0.0005f, Kp=0.05f, Kd=1.5f, v=0.80f; |
Maximousse | 1:6d8c50dcae28 | 77 | #endif |
Maximousse | 1:6d8c50dcae28 | 78 | m3pi.sensor_auto_calibrate(); |
Maximousse | 1:6d8c50dcae28 | 79 | wait(1.0); |
Maximousse | 1:6d8c50dcae28 | 80 | bt.printf("---> While in<--- "); |
Maximousse | 1:6d8c50dcae28 | 81 | Timer t; t.start(); |
Maximousse | 1:6d8c50dcae28 | 82 | while(!done) |
Maximousse | 1:6d8c50dcae28 | 83 | { |
Maximousse | 1:6d8c50dcae28 | 84 | idx++; |
Maximousse | 1:6d8c50dcae28 | 85 | getCapt(); |
Maximousse | 1:6d8c50dcae28 | 86 | // PID |
Maximousse | 1:6d8c50dcae28 | 87 | l_k1 = l_k; |
Maximousse | 1:6d8c50dcae28 | 88 | l_k = m3pi.line_position(); |
Maximousse | 1:6d8c50dcae28 | 89 | //v = sp; |
Maximousse | 1:6d8c50dcae28 | 90 | omega = l_k*Kp; // Proportionnal |
Maximousse | 1:6d8c50dcae28 | 91 | sum += l_k; |
Maximousse | 1:6d8c50dcae28 | 92 | omega += sum*Ki; // Integral |
Maximousse | 1:6d8c50dcae28 | 93 | omega += (l_k-l_k1)*Kd; |
Maximousse | 1:6d8c50dcae28 | 94 | //omega *= v; |
Maximousse | 1:6d8c50dcae28 | 95 | |
Maximousse | 1:6d8c50dcae28 | 96 | |
Maximousse | 1:6d8c50dcae28 | 97 | vl = v+omega < vmax ? (v+omega > -vmax ? v+omega : vmax) : vmax; |
Maximousse | 1:6d8c50dcae28 | 98 | vr = v-omega < vmax ? (v-omega > -vmax ? v-omega : -vmax): vmax; |
Maximousse | 1:6d8c50dcae28 | 99 | |
Maximousse | 1:6d8c50dcae28 | 100 | |
Maximousse | 1:6d8c50dcae28 | 101 | //------------------------------------- Pour les zigzags |
Maximousse | 1:6d8c50dcae28 | 102 | //getCurrentSerial().printf("? hola que pasa ?"); |
Maximousse | 1:6d8c50dcae28 | 103 | |
Maximousse | 1:6d8c50dcae28 | 104 | //------------------------------------- |
Maximousse | 1:6d8c50dcae28 | 105 | |
Maximousse | 1:6d8c50dcae28 | 106 | //-----------------Intersection |
Maximousse | 1:6d8c50dcae28 | 107 | if(sensors[0] || sensors[4]) |
Maximousse | 1:6d8c50dcae28 | 108 | { |
Maximousse | 1:6d8c50dcae28 | 109 | done = true; |
Maximousse | 1:6d8c50dcae28 | 110 | } |
Maximousse | 1:6d8c50dcae28 | 111 | set_motors(vr,vl); |
Maximousse | 1:6d8c50dcae28 | 112 | } |
Maximousse | 1:6d8c50dcae28 | 113 | bt.printf("-->While out<--"); |
Maximousse | 1:6d8c50dcae28 | 114 | |
Maximousse | 1:6d8c50dcae28 | 115 | return; |
Maximousse | 1:6d8c50dcae28 | 116 | } |
Maximousse | 1:6d8c50dcae28 | 117 | |
Maximousse | 1:6d8c50dcae28 | 118 | |
Maximousse | 1:6d8c50dcae28 | 119 | |
Maximousse | 1:6d8c50dcae28 | 120 | |
Maximousse | 1:6d8c50dcae28 | 121 | |
Maximousse | 1:6d8c50dcae28 | 122 | |
Maximousse | 1:6d8c50dcae28 | 123 | |
Maximousse | 1:6d8c50dcae28 | 124 | |
Maximousse | 1:6d8c50dcae28 | 125 | |
Maximousse | 1:6d8c50dcae28 | 126 | void assignDirection(char c, float s){ |
Maximousse | 1:6d8c50dcae28 | 127 | if (c == 'a') m3pi.left(s); |
Maximousse | 1:6d8c50dcae28 | 128 | if (c == 'c') m3pi.right(s); |
Maximousse | 1:6d8c50dcae28 | 129 | if (c == 'd') { |
Maximousse | 1:6d8c50dcae28 | 130 | m3pi.right(s); |
Maximousse | 1:6d8c50dcae28 | 131 | wait(0.355); |
Maximousse | 1:6d8c50dcae28 | 132 | } |
Maximousse | 1:6d8c50dcae28 | 133 | wait(0.355); |
Maximousse | 1:6d8c50dcae28 | 134 | } |
Maximousse | 1:6d8c50dcae28 | 135 | |
Maximousse | 1:6d8c50dcae28 | 136 | |
Maximousse | 1:6d8c50dcae28 | 137 | |
Maximousse | 1:6d8c50dcae28 | 138 | |
mbed_official | 0:879aa9d0247b | 139 | int main() { |
Maximousse | 1:6d8c50dcae28 | 140 | bt.baud(115200); |
Maximousse | 1:6d8c50dcae28 | 141 | char c = 'a'; |
Maximousse | 1:6d8c50dcae28 | 142 | bt.printf("Hello World!\n"); |
mbed_official | 0:879aa9d0247b | 143 | while(1) { |
Maximousse | 1:6d8c50dcae28 | 144 | m3pi.stop(); |
Maximousse | 1:6d8c50dcae28 | 145 | c = bt.getc(); |
Maximousse | 1:6d8c50dcae28 | 146 | bt.printf("Je viens de recevoir : %c \n", c); |
Maximousse | 1:6d8c50dcae28 | 147 | //move |
Maximousse | 1:6d8c50dcae28 | 148 | assignDirection(c, 0.2); |
Maximousse | 1:6d8c50dcae28 | 149 | getCapt(); |
Maximousse | 1:6d8c50dcae28 | 150 | suiviIntersect(); |
mbed_official | 0:879aa9d0247b | 151 | } |
Maximousse | 1:6d8c50dcae28 | 152 | } |
Maximousse | 1:6d8c50dcae28 | 153 |