Revision 40:83ce8d1072ef, committed 2015-03-17

Comitter:

Jagang

Date:

Tue Mar 17 14:38:54 2015 +0000

Parent:

39:09c04fd42c94

Child:

41:c04c2ec37aad

Commit message:

Debug moteur

Changed in this revision

--- a/Asservissement/Motor.h	Mon Mar 16 21:46:12 2015 +0000
+++ b/Asservissement/Motor.h	Tue Mar 17 14:38:54 2015 +0000
@@ -6,8 +6,8 @@
 class Motor
 {
     private:
-        PwmOut* pwm;
-        DigitalOut* dir;
+        PwmOut pwm;
+        DigitalOut dir;
         bool inverse;
         float speed;
         
@@ -17,16 +17,16 @@
          * @param pwm Pin sur laquelle sort la PWM
          * @param dir Pin sur laquelle sort le bit de direction
          */
-        Motor(PwmOut* pwm, DigitalOut* dir)
+        Motor(PinName pin_pwm, PinName pin_dir): pwm(pin_pwm), dir(pin_dir)
         {
-            this->pwm = pwm;
-            this->dir = dir;
             inverse = false;
             speed = 0.0f;
-            this->pwm->period_us(10);    
+            pwm.period_us(10);  
+            pwm = 0.0f;  
             
         }
-        virtual ~Motor();
+        
+        virtual ~Motor(){}
 
         /** Fixe la vitesse du moteur
          *
@@ -34,16 +34,13 @@
          */
         void setSpeed(float s)
         {
-            if(s<= 1.0f && s >=-1.0f)
-            {
-                speed = fabs_(s);   
-                inverse = (s <= 0.0f);
-            }
+            float abs = fabs_(s);
+            speed = s;
+            pwm = abs;
+            if(s > 0)
+                dir = !inverse;
             else
-            {
-                speed = 0.0f;
-                inverse = false;
-            }    
+                dir = inverse;
         }
 
         /** Retourne la vitesse théorique du moteur
@@ -72,17 +69,6 @@
          *
          * @return True ou False
          */
-        bool setInverse(){return inverse;}    
+        bool getInverse(){return inverse;}    
         
-        void update(float s)
-        {
-            this->setSpeed(s);
-            
-            if(inverse)
-                dir->write(1);
-            else
-                dir->write(0);
-                
-            pwm->write(speed);   
-        }
 };
\ No newline at end of file

--- a/defines.h	Mon Mar 16 21:46:12 2015 +0000
+++ b/defines.h	Tue Mar 17 14:38:54 2015 +0000
@@ -1,5 +1,5 @@
 
-#define OUT_USB
+//#define OUT_USB
 
 #ifdef OUT_USB
     #define OUT_TX USBTX
@@ -9,5 +9,12 @@
     #define OUT_RX PA_12
 #endif
 
+#define PWM_MOT1 PB_13
+#define PWM_MOT2 PB_14
+#define PWM_MOT3 PB_15
+
+#define DIR_MOT1 PC_9
+#define DIR_MOT2 PB_8
+#define DIR_MOT3 PB_9
 
 

--- a/main.cpp	Mon Mar 16 21:46:12 2015 +0000
+++ b/main.cpp	Tue Mar 17 14:38:54 2015 +0000
@@ -43,14 +43,14 @@
     */
     //nucleo
     
-    PwmOut pw1(PB_13);
-    DigitalOut dir1(PC_9);
-    PwmOut pw2(PB_14);
-    DigitalOut dir2(PB_8);
+    PwmOut pw1(PWM_MOT1);
+    DigitalOut dir1(DIR_MOT1);
+    PwmOut pw2(PWM_MOT2);
+    DigitalOut dir2(DIR_MOT2);
     
         
-    Motor motorR(&pw1,&dir1);
-    Motor motorL(&pw2,&dir2);    
+    Motor motorR(PWM_MOT1,DIR_MOT1);
+    Motor motorL(PWM_MOT2,DIR_MOT2);    
     logger.printf("mise a jour des pwm.\r\n");
     Timer t;
     
@@ -70,22 +70,22 @@
     
     test.SetMode(0); // Position
     
-    char dataOff[] = {0};
+    /*char dataOff[] = {0};
     char dataOn[] = {1};
-    /*while(1)
+    while(1)
     {
-        logger.printf("0: %f\r\n",ain0);
-        logger.printf("1: %f\r\n",ain1);
-        logger.printf("2: %f\r\n",ain2);
-        logger.printf("3: %f\r\n",ain3);
-        logger.printf("4: %f\r\n\r\n",ain4);
+        logger.printf("0: %f\r\n",ain0.read()*3.3);
+        logger.printf("1: %f\r\n",ain1.read()*3.3);
+        logger.printf("2: %f\r\n",ain2.read()*3.3);
+        logger.printf("3: %f\r\n",ain3.read()*3.3);
+        logger.printf("4: %f\r\n\r\n",ain4.read()*3.3);
         led = !led;
-        wait(0.5);
-        test.write(0x05,0x19,1,dataOff);
-        wait(0.5);
-        test.write(0x05,0x19,1,dataOn);
+        wait(1);
+        motorR.setSpeed(1.0);
+        wait(1);
+        motorR.setSpeed(0.0);
     }*/
-    bool testOdo = true;
+    bool testOdo = false;
     
     
     Asserv<float> instanceAsserv(&odometry);
@@ -93,13 +93,18 @@
     /*----------------------------------------------------------------------------------------------*/
     instanceAsserv.setGoal( (float)10,(float)0, (float)PI/2);
     
+    logger.printf("Debut boucle.\r\n");
+    
     while(1)
     {       
         //Asservissement :
-        t.start();         
+        logger.printf("1\r\n");
+        t.start();
+        logger.printf("2\r\n");
         instanceAsserv.update((float)t.read());
-                        
+        logger.printf("2\r\n");
         Mat<float> X( instanceAsserv.getX() );
+        logger.printf("3\r\n");
         float phi_r = instanceAsserv.getPhiR();
         float phi_l = instanceAsserv.getPhiL();
         float phi_max = instanceAsserv.getPhiMax();
@@ -109,15 +114,7 @@
         //(instanceAsserv.getX()).afficherMblue();
         //blue.printf("Odometry : \n\r");
         //z.afficherMblue();       
-                                 
-        //Mise a jour des moteurs :
-        if(!testOdo)
-        {
-            motorR.update((float)phi_r/phi_max);        
-            motorL.update((float)phi_l/phi_max);
-        }            
-        logger.printf("\n\n----------------- Commandes moteurs : mises a jour. ------------------ \n");
-                
+        
         //Timer Handling :
         t.stop();
         logger.printf("%f s ecoulee.\n\r", t.read());