Diff: main.cpp

Revision:

36:54f86bc6fd80

Parent:

35:54b13e154801

Child:

37:41abbd7eaec1

diff -r 54b13e154801 -r 54f86bc6fd80 main.cpp
--- a/main.cpp	Sun Mar 08 15:15:59 2015 +0000
+++ b/main.cpp	Sun Mar 08 21:24:27 2015 +0000
@@ -3,6 +3,7 @@
 #include "Map.h"
 
 #include "Asserv.h"
+#include "Motor.h"
 /*----------------------------------------------------------------------------------------------*/
     /*Serial*/    
     Serial pcs(USBTX, USBRX); // tx, rx
@@ -13,7 +14,7 @@
 
 int main()
 {
-    pcs.printf("demarrage");
+    pcs.printf("Initialisation...");
     //mbed
     /*
     PwmOut pw1(p22);
@@ -37,20 +38,15 @@
     DigitalOut dir2(D13);
     */
     //nucleo
+    
     PwmOut pw1(PA_0);
     DigitalOut dir1(PB_8);
     PwmOut pw2(PA_1);
     DigitalOut dir2(PB_9);
-    pw1.period_us(10);
-    pw2.period_us(10);
     
-    
-    dir1.write(0);
-    dir2.write(0);    
-    
-    pw1.write(0.0);
-    pw2.write(0.0);
-    //setPWM(&pw1,0.9);
+        
+    Motor motorR(&pw1,&dir1);
+    Motor motorL(&pw2,&dir2);    
     pcs.printf("mise a jour des pwm.\n");
     Timer t;    
     
@@ -60,73 +56,43 @@
     QEI qei_right(D4,D5,NC,1024*4,QEI::X4_ENCODING);
     Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,280);
     bool testOdo = true;
+    
+    
     Asserv<float> instanceAsserv(&odometry);
     
     /*----------------------------------------------------------------------------------------------*/
+    instanceAsserv.setGoal( (float)10,(float)0, (float)PI/2);
     
     while(1)
     {       
-        t.start(); 
-        //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);                                
-        instanceAsserv.update();        
-        //pcs.printf("command : \n phi_r = %f \n phi_l = %f \n", ((double)phi_r/phi_max), ((double)phi_l/phi_max));
-        //(instance.getCommand()).afficher();    
-        //blue.printf("State : \n\r");
-        //(instance.getX()).afficherMblue();
-        //blue.printf("Odometry : \n\r");
-        //z.afficherMblue();
+        //Asservissement :
+        t.start();         
+        instanceAsserv.update((float)t.read());
+                        
         Mat<float> X( instanceAsserv.getX() );
         float phi_r = instanceAsserv.getPhiR();
         float phi_l = instanceAsserv.getPhiL();
         float phi_max = instanceAsserv.getPhiMax();
         blue.printf(" x : %f ;\t y : %f ;\t theta : %f ;\t phiD = %f ;\t phiG = %f \n\r",X.get(1,1),X.get(2,1),X.get(3,1), X.get(4,1), X.get(5,1) );
-        //pcs.printf("Sigma : \n\r");
-        //(instance.getSigma()).afficher();
-        //pcs.printf("Kalman Gain : \n\r");
-        //(instance.getKGain()).afficherM();
-        
-        
-        if(phi_r >= 0)
-            dir1.write(1);
-        else
-            dir1.write(0);
-            
-        if(phi_l <= 0)
-            dir2.write(0);
-        else
-            dir2.write(1);
-            
+        //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);                                            
+        //blue.printf("State : \n\r");
+        //(instanceAsserv.getX()).afficherMblue();
+        //blue.printf("Odometry : \n\r");
+        //z.afficherMblue();       
+                                 
+        //Mise a jour des moteurs :
         if(!testOdo)
-        {    
-            if(abs(phi_r/phi_max) <= 1.0)
-            {
-                //pcs.printf( "VALUE PWM 1 : %f \n", (float)abs((double)phi_r/phi_max)) ;
-                setPWM(&pw1, (float)abs((double)phi_r/phi_max));
-            }
-            else
-            {
-                pcs.printf("P1...");
-                setPWM(&pw1, (float)abs((double)phi_max/phi_max));
-            }
+        {
+            motorR.update((float)phi_r/phi_max);        
+            motorL.update((float)phi_l/phi_max);
+        }            
+        pcs.printf("\n\n----------------- Commandes moteurs : mises a jour. ------------------ \n");
                 
-            if(abs(phi_l/phi_max) <= 1.0)
-            {
-                //pcs.printf( "VALUE PWM 2 : %f \n", (float)abs((double)phi_l/phi_max)) ;
-                setPWM(&pw2,(float)abs((double)phi_l/phi_max));
-            }
-            else
-            {
-                pcs.printf("P2...");
-                setPWM(&pw2, (float)abs((double)phi_max/phi_max));
-            }
-        }
-            
-        //pcs.printf("\n\n----------------- Commande mise executee. ------------------ \n");
+        //Timer Handling :
         t.stop();
-        pcs.printf("%f s \n\r", t.read());
+        pcs.printf("%f s ecoulee.\n\r", t.read());
         t.reset();
-        t.start();
-                
+        t.start();                
     }