dernier TP robot laby m3pi

Dependencies:   mbed FileSystem_POPS m3pi

main3.cpp

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

File content as of revision 7:a7bf354ae965:


#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[500];
unsigned short intercount;

unsigned lvwait,lwait2;
int main() {
    unsigned short i=0,ii;
    static char automain=0;
    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(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=0x09;
                        default :
                                break;
                    }
                    
  /*                  if(i<MAXSAVE){
                            sauvetab[i][0]=automain;
                            sauvetab[i][1]=statcapt;
                            sauvetab[i][2]=LFRvect;
                            sauvetab[i][3]=automr;
                            i=i+1;
                    }
    */                
              }
              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);
             }
             
                    
    }
    
}