dernier TP robot laby m3pi

Dependencies:   mbed FileSystem_POPS m3pi

main3.cpp

Committer:
bouaziz
Date:
4 months ago
Revision:
5:0f4a460521be
Parent:
4:dcad3508bdfb
Child:
6:f7c38d092b97

File content as of revision 5:0f4a460521be:


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

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"); 
Timer tm1;
Ticker tic1;

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;

// 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]);
    }
    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];
char flag1sec;

int main() {
    
 //   FILE *p= fopen("/fs/tt.txt","a+");
    m3pi.sensor_auto_calibrate();
    wait(1.);
    tic1.attach(&inter1,0.01);
    m3pi.cls();
  //  fprintf(p,"ecrire dans la cle USB\r\n");
  //  fclose(p);
    
    while(1) {
    // exemple de code 
              if(flag10ms){
                    flag10ms=0;
                    statcapt=lecture_captsol(tabsensor);
                    count++;
                    if(count>=100) { // on arrive à 1 seconde
                            count=0;
                            flag1sec=1;
                    }
              }
              // 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));
              }
    }
    
}