Diff: main3.cpp

Revision:

5:0f4a460521be

Parent:

4:dcad3508bdfb

--- a/main3.cpp	Wed Jan 10 08:51:51 2018 +0000
+++ b/main3.cpp	Mon Dec 17 17:53:23 2018 +0000
@@ -28,44 +28,45 @@
 unsigned short tabsensor[5];
 #define seuil(x) (x>400 ? 1 : 0)
 unsigned char statcapt;
-char PIDf(char commande){
-    unsigned char i;
-    m3pi.calibrated_sensors(tabsensor);
-    statcapt=0;
-    for(i=0;i<5;i++){
-            statcapt = (statcapt <<1)  | seuil(tabsensor[i]);
+
+// 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]);
     }
-    if(commande == 1) {
-            if((statcapt==0 )||(statcapt==0x1F )) {
-                        m3pi.left_motor(0.0);
-                        m3pi.right_motor(0.0); 
-                        myleds=0xF;
-                        return 0;   
-            }else{
-                    // 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);
-                }
-        }
-        return 1;
+    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;
@@ -73,39 +74,39 @@
         flag10ms=1;
 }
 
-int v;
+int v,count;
 unsigned char delai600ms;
 char chain[10];
+char flag1sec;
+
 int main() {
-    resetxbee =0;
-    wait(0.01);
-    resetxbee =1;
     
  //   FILE *p= fopen("/fs/tt.txt","a+");
     m3pi.sensor_auto_calibrate();
     wait(1.);
-        tic1.attach(&inter1,0.01);
+    tic1.attach(&inter1,0.01);
+    m3pi.cls();
   //  fprintf(p,"ecrire dans la cle USB\r\n");
   //  fclose(p);
     
     while(1) {
-            if(flag10ms==1){
+    // exemple de code 
+              if(flag10ms){
                     flag10ms=0;
-                    tm1.reset();
-                    tm1.start();
-                    PIDf(1);
-                    tm1.stop();
-                    v=tm1.read_us();
-                    delai600ms++;
+                    statcapt=lecture_captsol(tabsensor);
+                    count++;
+                    if(count>=100) { // on arrive à 1 seconde
+                            count=0;
+                            flag1sec=1;
+                    }
               }
-              if(delai600ms>=60){
-                    sprintf(chain,"%5u",v);     
-                    m3pi.locate(0,0);
-                    m3pi.print(chain,strlen(chain));
-                    delai600ms=0;
-               }
-            //pc.printf("%u %u %u %u %u\r\n",seuil(tabsensor[0]),seuil(tabsensor[1]),
-            //                                    seuil(tabsensor[2]),seuil(tabsensor[3]),seuil(tabsensor[4]));
+              // 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));
+              }
     }
     
 }