Programme de controle du robot joueur
Dependencies: m3piBluetooth mbed
Fork of Serial_HelloWorld_Mbed by
main.cpp
- Committer:
- Maximousse
- Date:
- 2018-05-03
- Revision:
- 1:6d8c50dcae28
- Parent:
- 0:879aa9d0247b
File content as of revision 1:6d8c50dcae28:
//-------------------------BT CONTROL #include "mbed.h" #include "m3pi.h" //#include "MSCFileSystem.h" Serial pc(USBTX, USBRX); //tx, rx Serial bt(p28,p27); //bluetooth DigitalOut myled1(LED1); DigitalOut myled2(LED2); DigitalOut myled3(LED3); DigitalOut myled4(LED4); DigitalOut leds [4] = {LED1, LED2, LED3, LED4}; m3pi m3pi; //MSCFileSystem fs("fs"); BusOut myleds(LED1, LED2, LED3, LED4); const int THRESH = 300; unsigned short sensors[5], mems[5], nbCapt, memsNb; char tab[4]; float position, proportional, last_proportional, integral; unsigned char dir; float speed, duration, s1, s2; void getCapt(bool mem = false) { int v = 0; nbCapt = 0; m3pi.calibrated_sensors(sensors); for(int i=0 ; i<5 ;i++) { sensors[i] = (sensors[i] > THRESH ? true : false); if(sensors[i]) { v += 0x1<<i; nbCapt++; } if(mem) { mems[i] = sensors[i]; memsNb = nbCapt; } } m3pi.leds(v); } void set_motors(float mr, float ml) { m3pi.left_motor(ml); m3pi.right_motor(mr); } void suiviIntersect(float intervalTour = 0.0f) { bt.printf("startSuivi_"); bool done = false; unsigned int idx=0; float vl, vr, omega, l_k=0, l_k1=0, sum=0, vmax=0.50f; #if 1 // Low speed const float Ki=0.005f, Kp=0.5f, Kd=1.5f, v=0.2; #else const float Ki=0.0005f, Kp=0.05f, Kd=1.5f, v=0.80f; #endif m3pi.sensor_auto_calibrate(); wait(1.0); bt.printf("---> While in<--- "); Timer t; t.start(); while(!done) { idx++; getCapt(); // PID l_k1 = l_k; l_k = m3pi.line_position(); //v = sp; omega = l_k*Kp; // Proportionnal sum += l_k; omega += sum*Ki; // Integral omega += (l_k-l_k1)*Kd; //omega *= v; vl = v+omega < vmax ? (v+omega > -vmax ? v+omega : vmax) : vmax; vr = v-omega < vmax ? (v-omega > -vmax ? v-omega : -vmax): vmax; //------------------------------------- Pour les zigzags //getCurrentSerial().printf("? hola que pasa ?"); //------------------------------------- //-----------------Intersection if(sensors[0] || sensors[4]) { done = true; } set_motors(vr,vl); } bt.printf("-->While out<--"); return; } void assignDirection(char c, float s){ if (c == 'a') m3pi.left(s); if (c == 'c') m3pi.right(s); if (c == 'd') { m3pi.right(s); wait(0.355); } wait(0.355); } int main() { bt.baud(115200); char c = 'a'; bt.printf("Hello World!\n"); while(1) { m3pi.stop(); c = bt.getc(); bt.printf("Je viens de recevoir : %c \n", c); //move assignDirection(c, 0.2); getCapt(); suiviIntersect(); } }