![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
exemple pour le TP SETI ROBOT M3PI
Dependencies: mbed FileSystem_POPS m3pi
Revision 5:0f4a460521be, committed 2018-12-17
- Comitter:
- bouaziz
- Date:
- Mon Dec 17 17:53:23 2018 +0000
- Parent:
- 4:dcad3508bdfb
- Commit message:
- exemple de code pour le TP SETI 2019
Changed in this revision
main3.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r dcad3508bdfb -r 0f4a460521be main3.cpp --- a/main3.cpp Wed Jan 10 08:51:51 2018 +0000 +++ b/main3.cpp Mon Dec 17 17:53:23 2018 +0000 @@ -28,44 +28,45 @@ unsigned short tabsensor[5]; #define seuil(x) (x>400 ? 1 : 0) unsigned char statcapt; -char PIDf(char commande){ - unsigned char i; - m3pi.calibrated_sensors(tabsensor); - statcapt=0; - for(i=0;i<5;i++){ - statcapt = (statcapt <<1) | seuil(tabsensor[i]); + +// fonction permet de lire les capteurs sol et de convertir cela sous la forme d'un octet +// seuls 5 bits sont utiles +// Vérifier l'ordre des bits sur la variable retrounée stat +// bit4 à bit0 de stat sachant que bit2 c'est le capteur milieu +unsigned char lecture_captsol(unsigned short *tab){ + unsigned char stat=0; + m3pi.calibrated_sensors(tab); + for(unsigned short ii=0;ii<5;ii++){ + stat = (stat <<1) | seuil(tab[ii]); } - if(commande == 1) { - if((statcapt==0 )||(statcapt==0x1F )) { - m3pi.left_motor(0.0); - m3pi.right_motor(0.0); - myleds=0xF; - return 0; - }else{ - // Get the position of the line. - current_pos_of_line = m3pi.line_position(); - proportional = current_pos_of_line; - // Compute the derivative - derivative = current_pos_of_line - previous_pos_of_line; - // Compute the integral - integral = (integral+ I_TERMO*proportional)/(1+I_TERMO); - // Remember the last position. - previous_pos_of_line = current_pos_of_line; - // Compute the power - power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; - // Compute new speeds - right = speed-(power*MAX); - left = speed+(power*MAX); - // limit checks on motor control - //MIN <right < MAX - // MIN <left < MAX - right = (right>MAX ? MAX :(right<MIN ? MIN : right)); - // send command to motors - m3pi.left_motor(left); - m3pi.right_motor(right); - } - } - return 1; + return stat; +} + +// Asservissement PID en flottant du robot. +// remplacer certains commentaires de limiteurs llimit checks on motor control +void PIDf(){ + + // Get the position of the line. + current_pos_of_line = m3pi.line_position(); + proportional = current_pos_of_line; + // Compute the derivative + derivative = current_pos_of_line - previous_pos_of_line; + // Compute the integral + integral = (integral+ I_TERMO*proportional)/(1+I_TERMO); + // Remember the last position. + previous_pos_of_line = current_pos_of_line; + // Compute the power + power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; + // Compute new speeds + right = speed-(power*MAX); + left = speed+(power*MAX); + // limit checks on motor control + //MIN <right < MAX + // MIN <left < MAX + right = (right>MAX ? MAX :(right<MIN ? MIN : right)); + // send command to motors + m3pi.left_motor(left); + m3pi.right_motor(right); } volatile char flag10ms; @@ -73,39 +74,39 @@ flag10ms=1; } -int v; +int v,count; unsigned char delai600ms; char chain[10]; +char flag1sec; + int main() { - resetxbee =0; - wait(0.01); - resetxbee =1; // FILE *p= fopen("/fs/tt.txt","a+"); m3pi.sensor_auto_calibrate(); wait(1.); - tic1.attach(&inter1,0.01); + tic1.attach(&inter1,0.01); + m3pi.cls(); // fprintf(p,"ecrire dans la cle USB\r\n"); // fclose(p); while(1) { - if(flag10ms==1){ + // exemple de code + if(flag10ms){ flag10ms=0; - tm1.reset(); - tm1.start(); - PIDf(1); - tm1.stop(); - v=tm1.read_us(); - delai600ms++; + statcapt=lecture_captsol(tabsensor); + count++; + if(count>=100) { // on arrive à 1 seconde + count=0; + flag1sec=1; + } } - if(delai600ms>=60){ - sprintf(chain,"%5u",v); - m3pi.locate(0,0); - m3pi.print(chain,strlen(chain)); - delai600ms=0; - } - //pc.printf("%u %u %u %u %u\r\n",seuil(tabsensor[0]),seuil(tabsensor[1]), - // seuil(tabsensor[2]),seuil(tabsensor[3]),seuil(tabsensor[4])); + // a vous de comprendre ce que fait ce code !!!!! + if((statcapt&0x04)&&flag1sec) { + flag1sec=0; + sprintf(chain,"%2x %5u",(unsigned)statcapt,(unsigned)tabsensor[2]); + m3pi.locate(0,0); + m3pi.print(chain,strlen(chain)); + } } }