Programme de controle du robot joueur
Dependencies: m3piBluetooth mbed
Fork of Serial_HelloWorld_Mbed by
Revision 1:6d8c50dcae28, committed 2018-05-03
- Comitter:
- Maximousse
- Date:
- Thu May 03 13:00:03 2018 +0000
- Parent:
- 0:879aa9d0247b
- Commit message:
- BT_Control_commit
Changed in this revision
m3pi.lib | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 879aa9d0247b -r 6d8c50dcae28 m3pi.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/m3pi.lib Thu May 03 13:00:03 2018 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/teams/Polybot1/code/m3piBluetooth/#b2df3579590b
diff -r 879aa9d0247b -r 6d8c50dcae28 main.cpp --- a/main.cpp Tue Feb 12 17:39:05 2013 +0000 +++ b/main.cpp Thu May 03 13:00:03 2018 +0000 @@ -1,10 +1,153 @@ + + + +//-------------------------BT CONTROL #include "mbed.h" - -Serial pc(USBTX, USBRX); // tx, rx - +#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() { - pc.printf("Hello World!\n"); + bt.baud(115200); + char c = 'a'; + bt.printf("Hello World!\n"); while(1) { - pc.putc(pc.getc() + 1); + m3pi.stop(); + c = bt.getc(); + bt.printf("Je viens de recevoir : %c \n", c); + //move + assignDirection(c, 0.2); + getCapt(); + suiviIntersect(); } -} \ No newline at end of file +} +