exemple pour le TP SETI ROBOT M3PI
Dependencies: mbed FileSystem_POPS m3pi
main3.cpp@4:dcad3508bdfb, 2018-01-10 (annotated)
- Committer:
- bouaziz
- Date:
- Wed Jan 10 08:51:51 2018 +0000
- Revision:
- 4:dcad3508bdfb
- Parent:
- 0:398afdd73d9e
- Child:
- 5:0f4a460521be
tp1 SETI veicule autonomes
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 | 4:dcad3508bdfb | 31 | char PIDf(char commande){ |
bouaziz | 4:dcad3508bdfb | 32 | unsigned char i; |
bouaziz | 4:dcad3508bdfb | 33 | m3pi.calibrated_sensors(tabsensor); |
bouaziz | 4:dcad3508bdfb | 34 | statcapt=0; |
bouaziz | 4:dcad3508bdfb | 35 | for(i=0;i<5;i++){ |
bouaziz | 4:dcad3508bdfb | 36 | statcapt = (statcapt <<1) | seuil(tabsensor[i]); |
bouaziz | 4:dcad3508bdfb | 37 | } |
bouaziz | 4:dcad3508bdfb | 38 | if(commande == 1) { |
bouaziz | 4:dcad3508bdfb | 39 | if((statcapt==0 )||(statcapt==0x1F )) { |
bouaziz | 4:dcad3508bdfb | 40 | m3pi.left_motor(0.0); |
bouaziz | 4:dcad3508bdfb | 41 | m3pi.right_motor(0.0); |
bouaziz | 4:dcad3508bdfb | 42 | myleds=0xF; |
bouaziz | 4:dcad3508bdfb | 43 | return 0; |
bouaziz | 4:dcad3508bdfb | 44 | }else{ |
bouaziz | 4:dcad3508bdfb | 45 | // Get the position of the line. |
bouaziz | 4:dcad3508bdfb | 46 | current_pos_of_line = m3pi.line_position(); |
bouaziz | 4:dcad3508bdfb | 47 | proportional = current_pos_of_line; |
bouaziz | 4:dcad3508bdfb | 48 | // Compute the derivative |
bouaziz | 4:dcad3508bdfb | 49 | derivative = current_pos_of_line - previous_pos_of_line; |
bouaziz | 4:dcad3508bdfb | 50 | // Compute the integral |
bouaziz | 4:dcad3508bdfb | 51 | integral = (integral+ I_TERMO*proportional)/(1+I_TERMO); |
bouaziz | 4:dcad3508bdfb | 52 | // Remember the last position. |
bouaziz | 4:dcad3508bdfb | 53 | previous_pos_of_line = current_pos_of_line; |
bouaziz | 4:dcad3508bdfb | 54 | // Compute the power |
bouaziz | 4:dcad3508bdfb | 55 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
bouaziz | 4:dcad3508bdfb | 56 | // Compute new speeds |
bouaziz | 4:dcad3508bdfb | 57 | right = speed-(power*MAX); |
bouaziz | 4:dcad3508bdfb | 58 | left = speed+(power*MAX); |
bouaziz | 4:dcad3508bdfb | 59 | // limit checks on motor control |
bouaziz | 4:dcad3508bdfb | 60 | //MIN <right < MAX |
bouaziz | 4:dcad3508bdfb | 61 | // MIN <left < MAX |
bouaziz | 4:dcad3508bdfb | 62 | right = (right>MAX ? MAX :(right<MIN ? MIN : right)); |
bouaziz | 4:dcad3508bdfb | 63 | // send command to motors |
bouaziz | 4:dcad3508bdfb | 64 | m3pi.left_motor(left); |
bouaziz | 4:dcad3508bdfb | 65 | m3pi.right_motor(right); |
bouaziz | 4:dcad3508bdfb | 66 | } |
bouaziz | 4:dcad3508bdfb | 67 | } |
bouaziz | 4:dcad3508bdfb | 68 | return 1; |
bouaziz | 4:dcad3508bdfb | 69 | } |
bouaziz | 4:dcad3508bdfb | 70 | |
bouaziz | 4:dcad3508bdfb | 71 | volatile char flag10ms; |
bouaziz | 4:dcad3508bdfb | 72 | void inter1(){ |
bouaziz | 4:dcad3508bdfb | 73 | flag10ms=1; |
bouaziz | 4:dcad3508bdfb | 74 | } |
bouaziz | 4:dcad3508bdfb | 75 | |
bouaziz | 4:dcad3508bdfb | 76 | int v; |
bouaziz | 4:dcad3508bdfb | 77 | unsigned char delai600ms; |
bouaziz | 4:dcad3508bdfb | 78 | char chain[10]; |
bouaziz | 0:398afdd73d9e | 79 | int main() { |
bouaziz | 0:398afdd73d9e | 80 | resetxbee =0; |
bouaziz | 0:398afdd73d9e | 81 | wait(0.01); |
bouaziz | 0:398afdd73d9e | 82 | resetxbee =1; |
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 | 4:dcad3508bdfb | 87 | tic1.attach(&inter1,0.01); |
bouaziz | 4:dcad3508bdfb | 88 | // fprintf(p,"ecrire dans la cle USB\r\n"); |
bouaziz | 4:dcad3508bdfb | 89 | // fclose(p); |
bouaziz | 0:398afdd73d9e | 90 | |
bouaziz | 0:398afdd73d9e | 91 | while(1) { |
bouaziz | 4:dcad3508bdfb | 92 | if(flag10ms==1){ |
bouaziz | 4:dcad3508bdfb | 93 | flag10ms=0; |
bouaziz | 4:dcad3508bdfb | 94 | tm1.reset(); |
bouaziz | 4:dcad3508bdfb | 95 | tm1.start(); |
bouaziz | 4:dcad3508bdfb | 96 | PIDf(1); |
bouaziz | 4:dcad3508bdfb | 97 | tm1.stop(); |
bouaziz | 4:dcad3508bdfb | 98 | v=tm1.read_us(); |
bouaziz | 4:dcad3508bdfb | 99 | delai600ms++; |
bouaziz | 4:dcad3508bdfb | 100 | } |
bouaziz | 4:dcad3508bdfb | 101 | if(delai600ms>=60){ |
bouaziz | 4:dcad3508bdfb | 102 | sprintf(chain,"%5u",v); |
bouaziz | 4:dcad3508bdfb | 103 | m3pi.locate(0,0); |
bouaziz | 4:dcad3508bdfb | 104 | m3pi.print(chain,strlen(chain)); |
bouaziz | 4:dcad3508bdfb | 105 | delai600ms=0; |
bouaziz | 4:dcad3508bdfb | 106 | } |
bouaziz | 4:dcad3508bdfb | 107 | //pc.printf("%u %u %u %u %u\r\n",seuil(tabsensor[0]),seuil(tabsensor[1]), |
bouaziz | 4:dcad3508bdfb | 108 | // seuil(tabsensor[2]),seuil(tabsensor[3]),seuil(tabsensor[4])); |
bouaziz | 0:398afdd73d9e | 109 | } |
bouaziz | 4:dcad3508bdfb | 110 | |
bouaziz | 0:398afdd73d9e | 111 | } |