dernier TP robot laby m3pi

Dependencies:   mbed FileSystem_POPS m3pi

Files at this revision

API Documentation at this revision

Comitter:
bouaziz
Date:
Mon Jan 21 10:22:27 2019 +0000
Parent:
5:0f4a460521be
Child:
7:a7bf354ae965
Commit message:
TP SETI Paris Saclay

Changed in this revision

main3.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/main3.cpp	Mon Dec 17 17:53:23 2018 +0000
+++ b/main3.cpp	Mon Jan 21 10:22:27 2019 +0000
@@ -5,12 +5,11 @@
 
 m3pi m3pi;                                  // Initialise the m3pi
 
-Serial xbee(p28,p27);
-DigitalOut resetxbee(p26);
 Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hanging
 //MSCFileSystem fs("fs"); 
+
+Ticker tic1;
 Timer tm1;
-Ticker tic1;
 
 BusOut myleds(LED1, LED2, LED3, LED4);
 
@@ -28,6 +27,7 @@
 unsigned short tabsensor[5];
 #define seuil(x) (x>400 ? 1 : 0)
 unsigned char statcapt;
+unsigned char LFRstat;
 
 // fonction permet de lire les capteurs sol et de convertir cela sous la forme d'un octet 
 // seuls 5 bits sont utiles
@@ -39,6 +39,7 @@
     for(unsigned short ii=0;ii<5;ii++){
             stat = (stat <<1)  | seuil(tab[ii]);
     }
+    LFRstat = ((stat&0x04)==0x04) ? 0x02 : 0) | ((stat&0x03)==0x03) ? 0x01 : 0)|((stat&0x18)==0x18) ? 0x04 : 0);
     return stat;
 }
 
@@ -77,35 +78,141 @@
 int v,count;
 unsigned char delai600ms;
 char chain[10];
-char flag1sec;
+volatile char flag1sec,flagmesure;
+char autom;
+unsigned int valtimer;
+
+void automate(){
+
+    switch(autom){
+        case 0 : m3pi.forward(0.3);
+                 autom=1;
+                break;
+        case 1 :
+                if((statcapt&0x11)==0x11){
+                    autom=2;
+                }
+                break;
+        case 2 :
+                if((statcapt&0x11)==0x00){
+                    autom=3;
+                    tm1.start();
+                }
+                break;        
+        case 3 :
+                if((statcapt&0x11)==0x11){
+                    autom=4;
+                    m3pi.stop();
+                    tm1.stop();
+                    valtimer=tm1.read_us();
+                    flagmesure=1;
+                }
+                break;
+        default:
+                break;
+    }    
+}
 
+char sensrotation=0; // 0    1 Left 2 Right
+char rotation_flag=0;
+void automrotation(){
+    static unsigned char autom=0;
+    static unsigned short vtime;
+    switch(automr){
+        case 0 : m3pi.stop();
+                 automr=1;
+                 vtime=0;
+                break;
+        case 1 :vtime++;
+                if(vtime>=25){
+                    switch(sensrotation){
+                        case 2 :        
+                                m3pi.right(0.3);
+                                break;
+                        case 1 :
+                                m3pi.left(0.3);
+                                break;
+                        default : 
+                                automr=0;
+                                break;
+                    }
+                    automr=2;
+                }
+                break; 
+        case 2 :
+                if(sensrotation){
+                    if((statcapt&1)&& (sensrotation==2)){
+                           automr=3;
+                    }
+                else{
+                    if((statcapt&0x10) && (sensrotation==1)){
+                           automr=3;
+                    }
+                }
+                break;        
+        case 3 :
+                if( ((sensrotation==2)&&(statcapt&0x06))||((sensrotation==1)&&(statcapt&0x0C)) ) {
+                    m3pi.stop();
+                    vtime=0;
+                    automr=4;
+                }
+                break;
+        case 4 :vtime++;
+                if(vtime>=30){
+                      rotation_flag=1;
+                      automr=0;  
+                }
+                break;                
+        default:
+                automr=0;
+                break;
+    }    
+}
+
+unsigned char inver;
+char trotate[2] = {1,2};
+char indexrotate=0;
 int main() {
-    
+    static char automain=0;
+//    myleds=0xF;
  //   FILE *p= fopen("/fs/tt.txt","a+");
+    wait(1.);
     m3pi.sensor_auto_calibrate();
     wait(1.);
+   //3pi.cls();
+    myleds=0xF;
     tic1.attach(&inter1,0.01);
-    m3pi.cls();
+    tm1.reset();
   //  fprintf(p,"ecrire dans la cle USB\r\n");
   //  fclose(p);
     
     while(1) {
     // exemple de code 
               if(flag10ms){
+                   // myleds=0;
                     flag10ms=0;
                     statcapt=lecture_captsol(tabsensor);
-                    count++;
-                    if(count>=100) { // on arrive à 1 seconde
-                            count=0;
-                            flag1sec=1;
+                    switch(automain){
+                        case 0 : if(LFRstat !=0){
+                                      PIDf();  
+                                 }else{
+                                      m3pi.stop();
+                                      automain=1;
+                                      sensrotation=trotate[indexrotate];
+                                      indexrotate = (indexrotate+1)&0x01;  //(indexrotate+1)%2;
+                                 }
+                                break;
+                        case 1 :
+                                automrotation();
+                                if(rotation_flag==1){
+                                        rotation_flag=0;
+                                        automain=0;
+                                }
+                                break;        
+                        default :
+                                break;
                     }
-              }
-              // a vous de comprendre ce que fait ce code !!!!!
-              if((statcapt&0x04)&&flag1sec) {
-                   flag1sec=0;
-                   sprintf(chain,"%2x %5u",(unsigned)statcapt,(unsigned)tabsensor[2]);     
-                   m3pi.locate(0,0);
-                   m3pi.print(chain,strlen(chain));
+                    
               }
     }