dernier TP robot laby m3pi

Dependencies:   mbed FileSystem_POPS m3pi

main3.cpp

Committer:
bouaziz
Date:
3 months ago
Revision:
6:f7c38d092b97
Parent:
5:0f4a460521be
Child:
7:a7bf354ae965

File content as of revision 6:f7c38d092b97:


#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"); 

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>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
// 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) ? 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
        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;
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);
    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);
                    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;
                    }
                    
              }
    }
    
}