dernier TP robot laby m3pi

Dependencies:   mbed FileSystem_POPS m3pi

Committer:
bouaziz
Date:
Mon Jan 21 10:22:27 2019 +0000
Revision:
6:f7c38d092b97
Parent:
5:0f4a460521be
Child:
7:a7bf354ae965
TP SETI Paris Saclay

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 pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging
bouaziz 4:dcad3508bdfb 9 //MSCFileSystem fs("fs");
bouaziz 6:f7c38d092b97 10
bouaziz 6:f7c38d092b97 11 Ticker tic1;
bouaziz 4:dcad3508bdfb 12 Timer tm1;
bouaziz 0:398afdd73d9e 13
bouaziz 0:398afdd73d9e 14 BusOut myleds(LED1, LED2, LED3, LED4);
bouaziz 0:398afdd73d9e 15
bouaziz 4:dcad3508bdfb 16 // all variables are float
bouaziz 4:dcad3508bdfb 17 #define D_TERM 0.0
bouaziz 4:dcad3508bdfb 18 #define I_TERMO 0.1
bouaziz 4:dcad3508bdfb 19 #define I_TERM 0.1
bouaziz 4:dcad3508bdfb 20 #define P_TERM 0.9
bouaziz 4:dcad3508bdfb 21 #define MAX 0.3
bouaziz 4:dcad3508bdfb 22 #define MIN -0.2
bouaziz 4:dcad3508bdfb 23 float current_pos_of_line,derivative,proportional,power,integral,right,left,previous_pos_of_line;
bouaziz 4:dcad3508bdfb 24 float speed =0.3;
bouaziz 4:dcad3508bdfb 25
bouaziz 4:dcad3508bdfb 26
bouaziz 4:dcad3508bdfb 27 unsigned short tabsensor[5];
bouaziz 4:dcad3508bdfb 28 #define seuil(x) (x>400 ? 1 : 0)
bouaziz 4:dcad3508bdfb 29 unsigned char statcapt;
bouaziz 6:f7c38d092b97 30 unsigned char LFRstat;
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 6:f7c38d092b97 42 LFRstat = ((stat&0x04)==0x04) ? 0x02 : 0) | ((stat&0x03)==0x03) ? 0x01 : 0)|((stat&0x18)==0x18) ? 0x04 : 0);
bouaziz 5:0f4a460521be 43 return stat;
bouaziz 5:0f4a460521be 44 }
bouaziz 5:0f4a460521be 45
bouaziz 5:0f4a460521be 46 // Asservissement PID en flottant du robot.
bouaziz 5:0f4a460521be 47 // remplacer certains commentaires de limiteurs llimit checks on motor control
bouaziz 5:0f4a460521be 48 void PIDf(){
bouaziz 5:0f4a460521be 49
bouaziz 5:0f4a460521be 50 // Get the position of the line.
bouaziz 5:0f4a460521be 51 current_pos_of_line = m3pi.line_position();
bouaziz 5:0f4a460521be 52 proportional = current_pos_of_line;
bouaziz 5:0f4a460521be 53 // Compute the derivative
bouaziz 5:0f4a460521be 54 derivative = current_pos_of_line - previous_pos_of_line;
bouaziz 5:0f4a460521be 55 // Compute the integral
bouaziz 5:0f4a460521be 56 integral = (integral+ I_TERMO*proportional)/(1+I_TERMO);
bouaziz 5:0f4a460521be 57 // Remember the last position.
bouaziz 5:0f4a460521be 58 previous_pos_of_line = current_pos_of_line;
bouaziz 5:0f4a460521be 59 // Compute the power
bouaziz 5:0f4a460521be 60 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
bouaziz 5:0f4a460521be 61 // Compute new speeds
bouaziz 5:0f4a460521be 62 right = speed-(power*MAX);
bouaziz 5:0f4a460521be 63 left = speed+(power*MAX);
bouaziz 5:0f4a460521be 64 // limit checks on motor control
bouaziz 5:0f4a460521be 65 //MIN <right < MAX
bouaziz 5:0f4a460521be 66 // MIN <left < MAX
bouaziz 5:0f4a460521be 67 right = (right>MAX ? MAX :(right<MIN ? MIN : right));
bouaziz 5:0f4a460521be 68 // send command to motors
bouaziz 5:0f4a460521be 69 m3pi.left_motor(left);
bouaziz 5:0f4a460521be 70 m3pi.right_motor(right);
bouaziz 4:dcad3508bdfb 71 }
bouaziz 4:dcad3508bdfb 72
bouaziz 4:dcad3508bdfb 73 volatile char flag10ms;
bouaziz 4:dcad3508bdfb 74 void inter1(){
bouaziz 4:dcad3508bdfb 75 flag10ms=1;
bouaziz 4:dcad3508bdfb 76 }
bouaziz 4:dcad3508bdfb 77
bouaziz 5:0f4a460521be 78 int v,count;
bouaziz 4:dcad3508bdfb 79 unsigned char delai600ms;
bouaziz 4:dcad3508bdfb 80 char chain[10];
bouaziz 6:f7c38d092b97 81 volatile char flag1sec,flagmesure;
bouaziz 6:f7c38d092b97 82 char autom;
bouaziz 6:f7c38d092b97 83 unsigned int valtimer;
bouaziz 6:f7c38d092b97 84
bouaziz 6:f7c38d092b97 85 void automate(){
bouaziz 6:f7c38d092b97 86
bouaziz 6:f7c38d092b97 87 switch(autom){
bouaziz 6:f7c38d092b97 88 case 0 : m3pi.forward(0.3);
bouaziz 6:f7c38d092b97 89 autom=1;
bouaziz 6:f7c38d092b97 90 break;
bouaziz 6:f7c38d092b97 91 case 1 :
bouaziz 6:f7c38d092b97 92 if((statcapt&0x11)==0x11){
bouaziz 6:f7c38d092b97 93 autom=2;
bouaziz 6:f7c38d092b97 94 }
bouaziz 6:f7c38d092b97 95 break;
bouaziz 6:f7c38d092b97 96 case 2 :
bouaziz 6:f7c38d092b97 97 if((statcapt&0x11)==0x00){
bouaziz 6:f7c38d092b97 98 autom=3;
bouaziz 6:f7c38d092b97 99 tm1.start();
bouaziz 6:f7c38d092b97 100 }
bouaziz 6:f7c38d092b97 101 break;
bouaziz 6:f7c38d092b97 102 case 3 :
bouaziz 6:f7c38d092b97 103 if((statcapt&0x11)==0x11){
bouaziz 6:f7c38d092b97 104 autom=4;
bouaziz 6:f7c38d092b97 105 m3pi.stop();
bouaziz 6:f7c38d092b97 106 tm1.stop();
bouaziz 6:f7c38d092b97 107 valtimer=tm1.read_us();
bouaziz 6:f7c38d092b97 108 flagmesure=1;
bouaziz 6:f7c38d092b97 109 }
bouaziz 6:f7c38d092b97 110 break;
bouaziz 6:f7c38d092b97 111 default:
bouaziz 6:f7c38d092b97 112 break;
bouaziz 6:f7c38d092b97 113 }
bouaziz 6:f7c38d092b97 114 }
bouaziz 5:0f4a460521be 115
bouaziz 6:f7c38d092b97 116 char sensrotation=0; // 0 1 Left 2 Right
bouaziz 6:f7c38d092b97 117 char rotation_flag=0;
bouaziz 6:f7c38d092b97 118 void automrotation(){
bouaziz 6:f7c38d092b97 119 static unsigned char autom=0;
bouaziz 6:f7c38d092b97 120 static unsigned short vtime;
bouaziz 6:f7c38d092b97 121 switch(automr){
bouaziz 6:f7c38d092b97 122 case 0 : m3pi.stop();
bouaziz 6:f7c38d092b97 123 automr=1;
bouaziz 6:f7c38d092b97 124 vtime=0;
bouaziz 6:f7c38d092b97 125 break;
bouaziz 6:f7c38d092b97 126 case 1 :vtime++;
bouaziz 6:f7c38d092b97 127 if(vtime>=25){
bouaziz 6:f7c38d092b97 128 switch(sensrotation){
bouaziz 6:f7c38d092b97 129 case 2 :
bouaziz 6:f7c38d092b97 130 m3pi.right(0.3);
bouaziz 6:f7c38d092b97 131 break;
bouaziz 6:f7c38d092b97 132 case 1 :
bouaziz 6:f7c38d092b97 133 m3pi.left(0.3);
bouaziz 6:f7c38d092b97 134 break;
bouaziz 6:f7c38d092b97 135 default :
bouaziz 6:f7c38d092b97 136 automr=0;
bouaziz 6:f7c38d092b97 137 break;
bouaziz 6:f7c38d092b97 138 }
bouaziz 6:f7c38d092b97 139 automr=2;
bouaziz 6:f7c38d092b97 140 }
bouaziz 6:f7c38d092b97 141 break;
bouaziz 6:f7c38d092b97 142 case 2 :
bouaziz 6:f7c38d092b97 143 if(sensrotation){
bouaziz 6:f7c38d092b97 144 if((statcapt&1)&& (sensrotation==2)){
bouaziz 6:f7c38d092b97 145 automr=3;
bouaziz 6:f7c38d092b97 146 }
bouaziz 6:f7c38d092b97 147 else{
bouaziz 6:f7c38d092b97 148 if((statcapt&0x10) && (sensrotation==1)){
bouaziz 6:f7c38d092b97 149 automr=3;
bouaziz 6:f7c38d092b97 150 }
bouaziz 6:f7c38d092b97 151 }
bouaziz 6:f7c38d092b97 152 break;
bouaziz 6:f7c38d092b97 153 case 3 :
bouaziz 6:f7c38d092b97 154 if( ((sensrotation==2)&&(statcapt&0x06))||((sensrotation==1)&&(statcapt&0x0C)) ) {
bouaziz 6:f7c38d092b97 155 m3pi.stop();
bouaziz 6:f7c38d092b97 156 vtime=0;
bouaziz 6:f7c38d092b97 157 automr=4;
bouaziz 6:f7c38d092b97 158 }
bouaziz 6:f7c38d092b97 159 break;
bouaziz 6:f7c38d092b97 160 case 4 :vtime++;
bouaziz 6:f7c38d092b97 161 if(vtime>=30){
bouaziz 6:f7c38d092b97 162 rotation_flag=1;
bouaziz 6:f7c38d092b97 163 automr=0;
bouaziz 6:f7c38d092b97 164 }
bouaziz 6:f7c38d092b97 165 break;
bouaziz 6:f7c38d092b97 166 default:
bouaziz 6:f7c38d092b97 167 automr=0;
bouaziz 6:f7c38d092b97 168 break;
bouaziz 6:f7c38d092b97 169 }
bouaziz 6:f7c38d092b97 170 }
bouaziz 6:f7c38d092b97 171
bouaziz 6:f7c38d092b97 172 unsigned char inver;
bouaziz 6:f7c38d092b97 173 char trotate[2] = {1,2};
bouaziz 6:f7c38d092b97 174 char indexrotate=0;
bouaziz 0:398afdd73d9e 175 int main() {
bouaziz 6:f7c38d092b97 176 static char automain=0;
bouaziz 6:f7c38d092b97 177 // myleds=0xF;
bouaziz 4:dcad3508bdfb 178 // FILE *p= fopen("/fs/tt.txt","a+");
bouaziz 6:f7c38d092b97 179 wait(1.);
bouaziz 0:398afdd73d9e 180 m3pi.sensor_auto_calibrate();
bouaziz 0:398afdd73d9e 181 wait(1.);
bouaziz 6:f7c38d092b97 182 //3pi.cls();
bouaziz 6:f7c38d092b97 183 myleds=0xF;
bouaziz 5:0f4a460521be 184 tic1.attach(&inter1,0.01);
bouaziz 6:f7c38d092b97 185 tm1.reset();
bouaziz 4:dcad3508bdfb 186 // fprintf(p,"ecrire dans la cle USB\r\n");
bouaziz 4:dcad3508bdfb 187 // fclose(p);
bouaziz 0:398afdd73d9e 188
bouaziz 0:398afdd73d9e 189 while(1) {
bouaziz 5:0f4a460521be 190 // exemple de code
bouaziz 5:0f4a460521be 191 if(flag10ms){
bouaziz 6:f7c38d092b97 192 // myleds=0;
bouaziz 4:dcad3508bdfb 193 flag10ms=0;
bouaziz 5:0f4a460521be 194 statcapt=lecture_captsol(tabsensor);
bouaziz 6:f7c38d092b97 195 switch(automain){
bouaziz 6:f7c38d092b97 196 case 0 : if(LFRstat !=0){
bouaziz 6:f7c38d092b97 197 PIDf();
bouaziz 6:f7c38d092b97 198 }else{
bouaziz 6:f7c38d092b97 199 m3pi.stop();
bouaziz 6:f7c38d092b97 200 automain=1;
bouaziz 6:f7c38d092b97 201 sensrotation=trotate[indexrotate];
bouaziz 6:f7c38d092b97 202 indexrotate = (indexrotate+1)&0x01; //(indexrotate+1)%2;
bouaziz 6:f7c38d092b97 203 }
bouaziz 6:f7c38d092b97 204 break;
bouaziz 6:f7c38d092b97 205 case 1 :
bouaziz 6:f7c38d092b97 206 automrotation();
bouaziz 6:f7c38d092b97 207 if(rotation_flag==1){
bouaziz 6:f7c38d092b97 208 rotation_flag=0;
bouaziz 6:f7c38d092b97 209 automain=0;
bouaziz 6:f7c38d092b97 210 }
bouaziz 6:f7c38d092b97 211 break;
bouaziz 6:f7c38d092b97 212 default :
bouaziz 6:f7c38d092b97 213 break;
bouaziz 5:0f4a460521be 214 }
bouaziz 6:f7c38d092b97 215
bouaziz 5:0f4a460521be 216 }
bouaziz 0:398afdd73d9e 217 }
bouaziz 4:dcad3508bdfb 218
bouaziz 0:398afdd73d9e 219 }