Diff: main3.cpp

Revision:

4:dcad3508bdfb

Parent:

0:398afdd73d9e

Child:

5:0f4a460521be

--- a/main3.cpp	Mon Nov 23 23:24:35 2015 +0000
+++ b/main3.cpp	Wed Jan 10 08:51:51 2018 +0000
@@ -8,24 +8,104 @@
 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"); 
+//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;
+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]);
+    }
+    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;
+}
+
+volatile char flag10ms;
+void inter1(){
+        flag10ms=1;
+}
+
+int v;
+unsigned char delai600ms;
+char chain[10];
 int main() {
     resetxbee =0;
     wait(0.01);
     resetxbee =1;
     
-    FILE *p= fopen("/fs/tt.txt","a+");
+ //   FILE *p= fopen("/fs/tt.txt","a+");
     m3pi.sensor_auto_calibrate();
     wait(1.);
-    
-    fprintf(p,"ecrire dans la cle USB\r\n");
-    fclose(p);
+        tic1.attach(&inter1,0.01);
+  //  fprintf(p,"ecrire dans la cle USB\r\n");
+  //  fclose(p);
     
     while(1) {
-        
-        
+            if(flag10ms==1){
+                    flag10ms=0;
+                    tm1.reset();
+                    tm1.start();
+                    PIDf(1);
+                    tm1.stop();
+                    v=tm1.read_us();
+                    delai600ms++;
+              }
+              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]));
     }
+    
 }