dernier TP robot laby m3pi

Dependencies:   mbed FileSystem_POPS m3pi

main3.cpp

Committer:
bouaziz
Date:
2 months ago
Revision:
8:6e2b5737c86c
Parent:
7:a7bf354ae965

File content as of revision 8:6e2b5737c86c:


#include "mbed.h"
#include "m3pi.h"
#include "MSCFileSystem.h"

m3pi m3pi;                                  // Initialise the m3pi

Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hanging
//MSCFileSystem fs("fs"); 

LocalFileSystem local("local");   

Ticker tic1;
Timer tm1;

BusOut myleds(LED1, LED2, LED3, LED4);

// all variables are float
#define D_TERM 0.0
#define I_TERMO 0.1
#define I_TERM  0.1
#define P_TERM  0.9
#define MAX 0.3
#define MIN -0.2
float current_pos_of_line,derivative,proportional,power,integral,right,left,previous_pos_of_line;
float speed =0.3;


unsigned short tabsensor[5];
#define seuil(x) (x>350 ? 1 : 0)
unsigned char statcapt;
unsigned char LFRstat, LFRvect;

// fonction permet de lire les capteurs sol et de convertir cela sous la forme d'un octet 
// seuls 5 bits sont utiles
// Vérifier l'ordre des bits sur la variable retrounée stat
// bit4 à bit0 de stat sachant que bit2 c'est le capteur milieu
unsigned char  lecture_captsol(unsigned short *tab){
    unsigned char stat=0;
    m3pi.calibrated_sensors(tab);
    for(unsigned short ii=0;ii<5;ii++){
            stat = (stat <<1)  | seuil(tab[ii]);
    }
    LFRstat = (((stat&0x04)==0x04)||((stat&0x03)==0x02)||((stat&0x18)==0x08) ? 0x02 : 0) | (((stat&0x03)==0x03) ? 0x01 : 0)|(((stat&0x18)==0x18) ? 0x04 : 0);
    return stat;
}

// Asservissement PID en flottant du robot.
// remplacer certains commentaires de limiteurs llimit checks on motor control
void PIDf(){

    // Get the position of the line.
        current_pos_of_line = m3pi.line_position();        
        proportional = current_pos_of_line;    
    // Compute the derivative
        derivative = current_pos_of_line - previous_pos_of_line;
    // Compute the integral
        integral = (integral+ I_TERMO*proportional)/(1+I_TERMO);
    // Remember the last position.
        previous_pos_of_line = current_pos_of_line;
    // Compute the power
        power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
    // Compute new speeds
        right = speed-(power*MAX);
        left  = speed+(power*MAX);
    // limit checks on motor control
         //MIN <right < MAX
        // MIN <left < MAX
        left = (left>MAX ? MAX :(left<MIN ? MIN : left));
        right = (right>MAX ? MAX :(right<MIN ? MIN : right));
    // send command to motors
        m3pi.left_motor(left);
        m3pi.right_motor(right);
}

volatile char flag10ms;
void inter1(){
        flag10ms=1;
}

int v,count;
unsigned char delai600ms;
char chain[10];
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;
static unsigned char automr=0;
void automrotation(){
    
    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;

#define MAXSAVE 4000
unsigned char sauvetab[MAXSAVE][3];

unsigned char texttab[300];
unsigned char TXT[20]={"FLFLLRLLRR"};
unsigned short intercount,index;

unsigned lvwait,lwait2;
char mess[20];

char rejoue(){
     static unsigned char automain2=0;
      switch(automain2){
                case 0 : PIDf();
                     switch(LFRvect) {
                          case 0x77 :
                                    automain2=37;
                                    lvwait=0;
                                    break;
                          case 0x00 : // pas bon
                                    m3pi.stop();
                                    m3pi.cls();
                                    sprintf(mess,"ERR0:%u",index);
                                    m3pi.print(mess,strlen(mess));
                                    while(1);
                                    //Indiquer probleme simplification
                                    break;
                          case 0x33 :
                                    automain2= 13;
                                    
                                    break;
                          case 0x66 :
                                    automain2= 13;
                                    
                                    break;
                          case 0x22 :
                                     
                                   // break;
                          default :
                                    break;
                        }          
                        break;
                case 2 : // fin rotation droite
                        if(LFRvect==0x22){
                                m3pi.stop();
                                automain2=0;
                        }
                        break; 
                case 13 : //inter droite ou tout droit
                        PIDf(); 
                        if((LFRvect ==0x00) || (LFRvect ==0x22)) {
                                switch(TXT[index]){
                                        case 'F' :
                                                    index++;
                                                    automain2 =0;
                                                    break;
                                        case 'R' :
                                                    //m3pi.stop();
                                                    lvwait=0;
                                                    index++;
                                                    if(LFRvect == 0x00){
                                                              m3pi.right(0.2);
                                                              automain2=2;
                                                    }else{
                                                                // alors c'est 0x22
                                                               automain2 =14;  //on tourne à droite                                                                                    }
                                                    break;
                                        case 'L' :
                                                    lvwait=0;
                                                    index++;
                                                    if(LFRvect == 0x00){
                                                              m3pi.left(0.2);
                                                              automain2=2;
                                                    }else{
                                                               automain2 =17;  //on tourne à droite
                                                    }                                                                                    }                                                           
                                                    break;
                                         default :
                                                    break;
                                }
                        }
                        break;
                                     
                case 14 :
                        lvwait++;
                        if(lvwait>(60/10)){
                                m3pi.stop();
                                automain=15;
                                lvwait=0;
                        }
                        break;
                       
                case 15 :
                        lvwait++;
                        if(lvwait>(500/10)){
                                m3pi.right(0.2);
                                automain=16;
                                lvwait=0;
                        }
                        break;
               case 16 :       
                        if((LFRvect&0x22)==0){
                                automain=2;
                        }
                        break;      
               case 17 :
                        lvwait++;
                        if(lvwait>(60/10)){
                                m3pi.stop();
                                automain2=18;
                                lvwait=0;
                        }
                        break;  
            
               case 18 :
                        lvwait++;
                        if(lvwait>(500/10)){
                                m3pi.left(0.2);
                                automain2=16;
                                lvwait=0;
                        }
                        break;                
                case 37 :
                        PIDf();
                        lvwait++;
                        if(LFRvect==0x77){
                                if((lvwait>15)){
                                    m3pi.stop();
                                    automain2=38;
                                    
                                }
                        }else{
                              
                                automain2= 13;
                                lvwait=0;   
                        }
                        break;
                case 38 :
                        myleds=0x0F;
                        return 1;
                default :
                        break;
    }
    return 0;
}


char explore(){
     static unsigned char automain=0;
      switch(automain){
            case 0 : PIDf(); 
                     switch(LFRvect) {
                          case 0x77 :
                                    automain=37;
                                    lvwait=0;
                                    break;
                          case 0x00 :
                                    //m3pi.stop();
                                    automain= 3;
                                    lvwait=0;
                                    texttab[intercount++]='B';
                                    break;
                          case 0x33 :
                                    automain= 13;
                                    texttab[intercount++]='R';
                                    break;
                          case 0x66 :
                                    automain= 26;
                                    
                                    break;
                          case 0x22 :
                                     
                                   // break;
                          default :
                                    break;
                    }          
                    break;
            case 3 : lvwait++;
                    if(lvwait>(60/10)){
                            m3pi.stop();
                            automain=1;
                    }
                    break;       
            case 1 : //lancer STOP avec rotation  à droite
                    lvwait++;
                    if(lvwait>(500/10)){
                            m3pi.right(0.2);
                            automain=2;
                    }
                    break;
            case 2 : // jattends pour STOPPER
                    if(LFRvect==0x22){
                        m3pi.stop();
                        automain=0;
                    }
                    break; 
            case 13 : //inter droite ou tout droit 
                    PIDf();  
                    switch(LFRvect){
                            case 0x00 :
                                        m3pi.stop();
                                        lvwait=0;
                                        automain =1;  
                            case 0x22 :
                                        //m3pi.stop();
                                        lvwait=0;
                                        automain =14;  
                            default :
                                    break;
                    }
                    break; 
                                  
            case 14 : 
                    lvwait++;
                    if(lvwait>(60/10)){
                            m3pi.stop();
                            automain=15;
                            lvwait=0;
                    }
                    break;
                    
            case 15 :
                    lvwait++;
                    if(lvwait>(500/10)){
                            m3pi.right(0.2);
                            automain=16;
                            lvwait=0;
                    }
                    break;
            case 16 :        
                    if((LFRvect&0x22)==0){
                            automain=2;
                    }
                    break;
            case 26 : // inter  gauche ou tout droit
                    PIDf();  
                    switch(LFRvect){
                            case 0x00 :
                                       // m3pi.stop();
                                        lvwait=0;
                                        automain =27; 
                                        texttab[intercount++]='L';
                                        break; 
                            case 0x22 : texttab[intercount++]='F';
                                        automain =0; 
                                        break; 
                            default :
                                    break;
                    }
                    break; 
            case 27 :
                    lvwait++;
                    if(lvwait>(50/10)){
                            m3pi.stop();
                            automain=28;
                            lvwait=0;
                    } 
                    break;      
            case 28 :
                    lvwait++;
                    if(lvwait>(500/10)){
                            m3pi.left(0.2);     
                            automain=2;
                    }
                    break;
            case 29 :
                    if(lvwait++>2){
                          m3pi.stop();
                          automain=0;
                    }
                    break;
            case 37 :
                    PIDf(); 
                    lvwait++;
                    if(LFRvect==0x77){
                            if((lvwait>15)){
                                m3pi.stop();
                                automain=38;
                            }
                    }else{
                            texttab[intercount++]='R';
                            automain= 13;
                            lvwait=0;    
                    }
                    break;
            case 38 :
                    myleds=0x0F;
                    return 1;
            default :
                    break;
        }
        return 0;                  
}


int main() {
    unsigned short i=0,ii;
    unsigned char supauto=2;
    myleds=0x0;
    FILE *p= fopen("/local/tt.txt","w");
    if(p!=0) {
        myleds=1;
    }
    wait(1.);
    m3pi.sensor_auto_calibrate();
    wait(1.);
   //3pi.cls();
    
    tic1.attach(&inter1,0.01);
    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);
                    LFRvect = (LFRvect <<4) + LFRstat;
                  /*   if(i<MAXSAVE){
                            sauvetab[i][0]=automain;
                            sauvetab[i][1]=LFRvect;
                            sauvetab[i][2]=statcapt;
                            i++;
                    }*/
                    switch(supauto){
                            case 0 :
                                    if(explore()){
                                                // simplification
                                                //attendre
                                                lvwait=0;
                                                supauto=1;
                                    break;
                            case 1 : 
                                    lvwait++;
                                    if(lvwait>=1500){
                                            myleds=0;
                                            supauto=2;
                                    break;
                            case 2 :
                                    if(rejoue()){
                                            supauto=3;
                                    }
                                    break;
                            case 3 :
                                    myleds(0x06);
                            default :
                             // se mettre en arret et afficher ereur
                                    break;    
                            
                    }
                    
   
              }
 /*             if(i>=MAXSAVE) {
                    m3pi.stop();
                    texttab[intercount++]=0;
                    fprintf(p,"%s\n",texttab);
                    for(i=0;i<MAXSAVE;i++){
                        fprintf(p,"%3u %02x %02x\n",(unsigned)sauvetab[i][0],(unsigned)sauvetab[i][1],(unsigned)sauvetab[i][2]);
                    }
                    myleds=0x0f;
                    fclose(p);
                    while(1);
             }*/
             
                    
    }
    
}