Polytech_TP_EES_M3PI / Mbed 2 deprecated m3PI_TP1_SETI2020

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?

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 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 }