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?
User | Revision | Line number | New 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 | } |