main3.cpp

00001 
00002 #include "mbed.h"
00003 #include "m3pi.h"
00004 #include "MSCFileSystem.h"
00005 
00006 m3pi m3pi;                                  // Initialise the m3pi
00007 
00008 Serial xbee(p28,p27);
00009 DigitalOut resetxbee(p26);
00010 Serial pc(USBTX, USBRX);                    // For debugging and pc messages, uses commented out to prevent hanging
00011 //MSCFileSystem fs("fs"); 
00012 Timer tm1;
00013 Ticker tic1;
00014 
00015 BusOut myleds(LED1, LED2, LED3, LED4);
00016 
00017 // all variables are float
00018 #define D_TERM 0.0
00019 #define I_TERMO 0.1
00020 #define I_TERM  0.1
00021 #define P_TERM  0.9
00022 #define MAX 0.3
00023 #define MIN -0.2
00024 float current_pos_of_line,derivative,proportional,power,integral,right,left,previous_pos_of_line;
00025 float speed =0.3;
00026 
00027 
00028 unsigned short tabsensor[5];
00029 #define seuil(x) (x>400 ? 1 : 0)
00030 unsigned char statcapt;
00031 
00032 // fonction permet de lire les capteurs sol et de convertir cela sous la forme d'un octet 
00033 // seuls 5 bits sont utiles
00034 // Vérifier l'ordre des bits sur la variable retrounée stat
00035 // bit4 à bit0 de stat sachant que bit2 c'est le capteur milieu
00036 unsigned char  lecture_captsol(unsigned short *tab){
00037     unsigned char stat=0;
00038     m3pi.calibrated_sensors(tab);
00039     for(unsigned short ii=0;ii<5;ii++){
00040             stat = (stat <<1)  | seuil(tab[ii]);
00041     }
00042     return stat;
00043 }
00044 
00045 // Asservissement PID en flottant du robot.
00046 // remplacer certains commentaires de limiteurs llimit checks on motor control
00047 void PIDf(){
00048 
00049     // Get the position of the line.
00050         current_pos_of_line = m3pi.line_position();        
00051         proportional = current_pos_of_line;    
00052     // Compute the derivative
00053         derivative = current_pos_of_line - previous_pos_of_line;
00054     // Compute the integral
00055         integral = (integral+ I_TERMO*proportional)/(1+I_TERMO);
00056     // Remember the last position.
00057         previous_pos_of_line = current_pos_of_line;
00058     // Compute the power
00059         power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
00060     // Compute new speeds
00061         right = speed-(power*MAX);
00062         left  = speed+(power*MAX);
00063     // limit checks on motor control
00064          //MIN <right < MAX
00065         // MIN <left < MAX
00066         right = (right>MAX ? MAX :(right<MIN ? MIN : right));
00067     // send command to motors
00068         m3pi.left_motor(left);
00069         m3pi.right_motor(right);
00070 }
00071 
00072 volatile char flag10ms;
00073 void inter1(){
00074         flag10ms=1;
00075 }
00076 
00077 int v,count;
00078 unsigned char delai600ms;
00079 char chain[10];
00080 char flag1sec;
00081 
00082 int main() {
00083     
00084  //   FILE *p= fopen("/fs/tt.txt","a+");
00085     m3pi.sensor_auto_calibrate();
00086     wait(1.);
00087     tic1.attach(&inter1,0.01);
00088     m3pi.cls();
00089   //  fprintf(p,"ecrire dans la cle USB\r\n");
00090   //  fclose(p);
00091     
00092     while(1) {
00093     // exemple de code 
00094               if(flag10ms){
00095                     flag10ms=0;
00096                     statcapt=lecture_captsol(tabsensor);
00097                     count++;
00098                     if(count>=100) { // on arrive à 1 seconde
00099                             count=0;
00100                             flag1sec=1;
00101                     }
00102               }
00103               // a vous de comprendre ce que fait ce code !!!!!
00104               if((statcapt&0x04)&&flag1sec) {
00105                    flag1sec=0;
00106                    sprintf(chain,"%2x %5u",(unsigned)statcapt,(unsigned)tabsensor[2]);     
00107                    m3pi.locate(0,0);
00108                    m3pi.print(chain,strlen(chain));
00109               }
00110     }
00111     
00112 }