The program
Dependencies: FileSystem_POPS Sound m3pi_test2 mbed
Fork of M3PI_SuiviLigne by
main3.cpp
- Committer:
- Maximousse
- Date:
- 2018-05-03
- Revision:
- 4:22275847875d
- Parent:
- 0:398afdd73d9e
File content as of revision 4:22275847875d:
#include "mbed.h" #include "m3pi.h" #include "MSCFileSystem.h" m3pi m3pi; // Initialise the m3pi DigitalOut resetxbee(p26); 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); 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; 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.0); 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(); } }