![](/media/cache/group/Logo_v4_ZnplGO9.jpg.50x50_q85.jpg)
il est la, il est beau!!!!
Dependencies: m3piBluetooth mbed
Diff: polulu.cpp
- Revision:
- 0:b839dbffc636
- Child:
- 1:63050abd40e9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/polulu.cpp Thu May 03 14:03:58 2018 +0000 @@ -0,0 +1,168 @@ +#include "mbed.h" +#include "m3pi.h" + +m3pi m3pi; // Initialise the m3pi +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}; +// variables + +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; + + +//------------------ paramétrage moteurs --------------------------// + + +void set_motors(float mr, float ml) +{ + m3pi.left_motor(ml); + m3pi.right_motor(mr); +} + +//---------------------- arret moteur -----------------------------// + +void stop_motors() +{ + m3pi.stop(); +} + + +//--------------- utilisation capteur -------------------------------// + +void getCapt(bool mem = false) +{ + int v = 0; + int 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); +} + + +//-------------------- suivi ligne00000000 -------------------------// + + +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; +} + + +//----------------- pilotage -> direction ---------------------// + + +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); +} + + +//-------- paramétrage fréquence (pour bluetooth ) --------// + +void deffreq(int x) +{ + bt.baud(x); +} + + +//---------- renvoi de message à l'utilisateur ------------// + + +void sendmsg(char* msg) +{ + bt.printf("%s\n", msg); +} + + +//-------- recuperation de l info sur la direction ---------// + + +void getdir(char c) +{ + c = bt.getc(); +} +