exemple pour le TP SETI ROBOT M3PI
Dependencies: mbed FileSystem_POPS m3pi
main3.cpp
- Committer:
- bouaziz
- Date:
- 2018-12-17
- Revision:
- 5:0f4a460521be
- Parent:
- 4:dcad3508bdfb
File content as of revision 5:0f4a460521be:
#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 //MSCFileSystem fs("fs"); Timer tm1; Ticker tic1; BusOut myleds(LED1, LED2, LED3, LED4); // all variables are float #define D_TERM 0.0 #define I_TERMO 0.1 #define I_TERM 0.1 #define P_TERM 0.9 #define MAX 0.3 #define MIN -0.2 float current_pos_of_line,derivative,proportional,power,integral,right,left,previous_pos_of_line; float speed =0.3; unsigned short tabsensor[5]; #define seuil(x) (x>400 ? 1 : 0) unsigned char statcapt; // 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]); } 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; void inter1(){ flag10ms=1; } int v,count; unsigned char delai600ms; char chain[10]; char flag1sec; int main() { // FILE *p= fopen("/fs/tt.txt","a+"); m3pi.sensor_auto_calibrate(); wait(1.); tic1.attach(&inter1,0.01); m3pi.cls(); // fprintf(p,"ecrire dans la cle USB\r\n"); // fclose(p); while(1) { // exemple de code if(flag10ms){ flag10ms=0; statcapt=lecture_captsol(tabsensor); count++; if(count>=100) { // on arrive à 1 seconde count=0; flag1sec=1; } } // 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)); } } }