il est la, il est beau!!!!
Dependencies: m3piBluetooth mbed
polulu.cpp
- Committer:
- thomasstrub
- Date:
- 2018-05-03
- Revision:
- 1:63050abd40e9
- Parent:
- 0:b839dbffc636
File content as of revision 1:63050abd40e9:
#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[0] = bt.getc(); }