Polytech_TP_EES_M3PI / Mbed 2 deprecated m3PI_TP1_SETI2020

exemple pour le TP SETI ROBOT M3PI

Dependencies:   mbed FileSystem_POPS m3pi

main3.cpp@5:0f4a460521be, 2018-12-17 (annotated)

Committer:
bouaziz
Date:
Mon Dec 17 17:53:23 2018 +0000
Revision:
5:0f4a460521be
Parent:
4:dcad3508bdfb 
exemple de code pour le TP SETI 2019

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bouaziz 0:398afdd73d9e 1
bouaziz 0:398afdd73d9e 2 #include "mbed.h"
bouaziz 0:398afdd73d9e 3 #include "m3pi.h"
bouaziz 0:398afdd73d9e 4 #include "MSCFileSystem.h"
bouaziz 0:398afdd73d9e 5
bouaziz 0:398afdd73d9e 6 m3pi m3pi; // Initialise the m3pi
bouaziz 0:398afdd73d9e 7
bouaziz 0:398afdd73d9e 8 Serial xbee(p28,p27);
bouaziz 0:398afdd73d9e 9 DigitalOut resetxbee(p26);
bouaziz 0:398afdd73d9e 10 Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging
bouaziz 4:dcad3508bdfb 11 //MSCFileSystem fs("fs");
bouaziz 4:dcad3508bdfb 12 Timer tm1;
bouaziz 4:dcad3508bdfb 13 Ticker tic1;
bouaziz 0:398afdd73d9e 14
bouaziz 0:398afdd73d9e 15 BusOut myleds(LED1, LED2, LED3, LED4);
bouaziz 0:398afdd73d9e 16
bouaziz 4:dcad3508bdfb 17 // all variables are float
bouaziz 4:dcad3508bdfb 18 #define D_TERM 0.0
bouaziz 4:dcad3508bdfb 19 #define I_TERMO 0.1
bouaziz 4:dcad3508bdfb 20 #define I_TERM 0.1
bouaziz 4:dcad3508bdfb 21 #define P_TERM 0.9
bouaziz 4:dcad3508bdfb 22 #define MAX 0.3
bouaziz 4:dcad3508bdfb 23 #define MIN -0.2
bouaziz 4:dcad3508bdfb 24 float current_pos_of_line,derivative,proportional,power,integral,right,left,previous_pos_of_line;
bouaziz 4:dcad3508bdfb 25 float speed =0.3;
bouaziz 4:dcad3508bdfb 26
bouaziz 4:dcad3508bdfb 27
bouaziz 4:dcad3508bdfb 28 unsigned short tabsensor[5];
bouaziz 4:dcad3508bdfb 29 #define seuil(x) (x>400 ? 1 : 0)
bouaziz 4:dcad3508bdfb 30 unsigned char statcapt;
bouaziz 5:0f4a460521be 31
bouaziz 5:0f4a460521be 32 // fonction permet de lire les capteurs sol et de convertir cela sous la forme d'un octet
bouaziz 5:0f4a460521be 33 // seuls 5 bits sont utiles
bouaziz 5:0f4a460521be 34 // Vérifier l'ordre des bits sur la variable retrounée stat
bouaziz 5:0f4a460521be 35 // bit4 à bit0 de stat sachant que bit2 c'est le capteur milieu
bouaziz 5:0f4a460521be 36 unsigned char lecture_captsol(unsigned short *tab){
bouaziz 5:0f4a460521be 37 unsigned char stat=0;
bouaziz 5:0f4a460521be 38 m3pi.calibrated_sensors(tab);
bouaziz 5:0f4a460521be 39 for(unsigned short ii=0;ii<5;ii++){
bouaziz 5:0f4a460521be 40 stat = (stat <<1) | seuil(tab[ii]);
bouaziz 4:dcad3508bdfb 41 }
bouaziz 5:0f4a460521be 42 return stat;
bouaziz 5:0f4a460521be 43 }
bouaziz 5:0f4a460521be 44
bouaziz 5:0f4a460521be 45 // Asservissement PID en flottant du robot.
bouaziz 5:0f4a460521be 46 // remplacer certains commentaires de limiteurs llimit checks on motor control
bouaziz 5:0f4a460521be 47 void PIDf(){
bouaziz 5:0f4a460521be 48
bouaziz 5:0f4a460521be 49 // Get the position of the line.
bouaziz 5:0f4a460521be 50 current_pos_of_line = m3pi.line_position();
bouaziz 5:0f4a460521be 51 proportional = current_pos_of_line;
bouaziz 5:0f4a460521be 52 // Compute the derivative
bouaziz 5:0f4a460521be 53 derivative = current_pos_of_line - previous_pos_of_line;
bouaziz 5:0f4a460521be 54 // Compute the integral
bouaziz 5:0f4a460521be 55 integral = (integral+ I_TERMO*proportional)/(1+I_TERMO);
bouaziz 5:0f4a460521be 56 // Remember the last position.
bouaziz 5:0f4a460521be 57 previous_pos_of_line = current_pos_of_line;
bouaziz 5:0f4a460521be 58 // Compute the power
bouaziz 5:0f4a460521be 59 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
bouaziz 5:0f4a460521be 60 // Compute new speeds
bouaziz 5:0f4a460521be 61 right = speed-(power*MAX);
bouaziz 5:0f4a460521be 62 left = speed+(power*MAX);
bouaziz 5:0f4a460521be 63 // limit checks on motor control
bouaziz 5:0f4a460521be 64 //MIN <right < MAX
bouaziz 5:0f4a460521be 65 // MIN <left < MAX
bouaziz 5:0f4a460521be 66 right = (right>MAX ? MAX :(right<MIN ? MIN : right));
bouaziz 5:0f4a460521be 67 // send command to motors
bouaziz 5:0f4a460521be 68 m3pi.left_motor(left);
bouaziz 5:0f4a460521be 69 m3pi.right_motor(right);
bouaziz 4:dcad3508bdfb 70 }
bouaziz 4:dcad3508bdfb 71
bouaziz 4:dcad3508bdfb 72 volatile char flag10ms;
bouaziz 4:dcad3508bdfb 73 void inter1(){
bouaziz 4:dcad3508bdfb 74 flag10ms=1;
bouaziz 4:dcad3508bdfb 75 }
bouaziz 4:dcad3508bdfb 76
bouaziz 5:0f4a460521be 77 int v,count;
bouaziz 4:dcad3508bdfb 78 unsigned char delai600ms;
bouaziz 4:dcad3508bdfb 79 char chain[10];
bouaziz 5:0f4a460521be 80 char flag1sec;
bouaziz 5:0f4a460521be 81
bouaziz 0:398afdd73d9e 82 int main() {
bouaziz 0:398afdd73d9e 83
bouaziz 4:dcad3508bdfb 84 // FILE *p= fopen("/fs/tt.txt","a+");
bouaziz 0:398afdd73d9e 85 m3pi.sensor_auto_calibrate();
bouaziz 0:398afdd73d9e 86 wait(1.);
bouaziz 5:0f4a460521be 87 tic1.attach(&inter1,0.01);
bouaziz 5:0f4a460521be 88 m3pi.cls();
bouaziz 4:dcad3508bdfb 89 // fprintf(p,"ecrire dans la cle USB\r\n");
bouaziz 4:dcad3508bdfb 90 // fclose(p);
bouaziz 0:398afdd73d9e 91
bouaziz 0:398afdd73d9e 92 while(1) {
bouaziz 5:0f4a460521be 93 // exemple de code
bouaziz 5:0f4a460521be 94 if(flag10ms){
bouaziz 4:dcad3508bdfb 95 flag10ms=0;
bouaziz 5:0f4a460521be 96 statcapt=lecture_captsol(tabsensor);
bouaziz 5:0f4a460521be 97 count++;
bouaziz 5:0f4a460521be 98 if(count>=100) { // on arrive à 1 seconde
bouaziz 5:0f4a460521be 99 count=0;
bouaziz 5:0f4a460521be 100 flag1sec=1;
bouaziz 5:0f4a460521be 101 }
bouaziz 4:dcad3508bdfb 102 }
bouaziz 5:0f4a460521be 103 // a vous de comprendre ce que fait ce code !!!!!
bouaziz 5:0f4a460521be 104 if((statcapt&0x04)&&flag1sec) {
bouaziz 5:0f4a460521be 105 flag1sec=0;
bouaziz 5:0f4a460521be 106 sprintf(chain,"%2x %5u",(unsigned)statcapt,(unsigned)tabsensor[2]);
bouaziz 5:0f4a460521be 107 m3pi.locate(0,0);
bouaziz 5:0f4a460521be 108 m3pi.print(chain,strlen(chain));
bouaziz 5:0f4a460521be 109 }
bouaziz 0:398afdd73d9e 110 }
bouaziz 4:dcad3508bdfb 111
bouaziz 0:398afdd73d9e 112 }