The program
Dependencies: FileSystem_POPS Sound m3pi_test2 mbed
Fork of m3PI_TP_POPS_II2015v0 by
Diff: main3.cpp
- Revision:
- 4:22275847875d
- Parent:
- 0:398afdd73d9e
--- a/main3.cpp Mon Nov 23 23:24:35 2015 +0000 +++ b/main3.cpp Thu May 03 12:08:03 2018 +0000 @@ -1,31 +1,166 @@ - #include "mbed.h" #include "m3pi.h" #include "MSCFileSystem.h" m3pi m3pi; // Initialise the m3pi -Serial xbee(p28,p27); DigitalOut resetxbee(p26); -Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging +DigitalOut led3(LED3); +DigitalOut led1(LED1); +DigitalIn push(p21); + +Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hangings +Serial bt(p28,p27); + MSCFileSystem fs("fs"); BusOut myleds(LED1, LED2, LED3, LED4); -int main() { - resetxbee =0; - wait(0.01); - resetxbee =1; +const int THRESH = 200; + +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; +char src='x'; + +Timer timer; + + + + + + + + + +Serial& getCurrentSerial() +{ + return (src == 'u' ? pc : bt); +} + + + + +void set_motors(float mr, float ml) +{ + m3pi.left_motor(ml); + m3pi.right_motor(mr); +} + +//------------------------------------------------------------------------------ + +void getCapt(bool mem = false) +{ + int v = 0; + nbCapt = 0; - FILE *p= fopen("/fs/tt.txt","a+"); + 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 suiviIntersect(float intervalTour = 0.0f) +{ + bool done = false; + //forward = true; + unsigned int idx=0; + float vl, vr, omega, l_k=0, l_k1=0, sum=0, vmax=1.0f; + +#if 1 // Low speed + const float Ki=0.007f, Kp=0.7f, Kd=1.5f, v=0.2; +#else + const float Ki=0.0007f, Kp=0.07f, Kd=1.5f, v=0.80f; +#endif m3pi.sensor_auto_calibrate(); - wait(1.); + wait(1.0); - fprintf(p,"ecrire dans la cle USB\r\n"); - fclose(p); - - while(1) { + 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; + vl = v+omega < vmax ? (v+omega > -vmax ? v+omega : vmax) : vmax; + vr = v-omega < vmax ? (v-omega > -vmax ? v-omega : -vmax): vmax; + + vr *= 1.2; + vl *= 1.2; + set_motors(vr,vl); + + //---------------------INTERSECTION + if(sensors[0] || sensors[4]) { + char c = 'a'; + //if ((c<='5') & (c>='1')) { + // leds[c - '1'] = !leds[c - '1']; + //} + bt.printf("Voici un %c \n", c); + } + + /* + if(forward) set_motors(vr,vl); + else set_motors(-vl,-vr); + */ + + m3pi.cls(); + //const char* C [1]= {'0' + idx}; + //m3pi.printf(C); } + + return; } + + + + + + + + + + +int main() { + bt.baud(115200); + m3pi.cls(); + wait(0.5); + + m3pi.sensor_auto_calibrate(); + + set_motors(0.f,0.f); + char c = 'a'; + bt.printf("Hello World!\n"); + + while(1) + { + getCapt(); + suiviIntersect(); + } +} \ No newline at end of file