Diff: MX12.cpp

Revision:

26:4632a02a8ef1

Parent:

24:2ab26ed08c83

Child:

27:06850c65b9c8

--- a/MX12.cpp	Sat Nov 13 20:05:30 2021 +0100
+++ b/MX12.cpp	Fri Nov 26 07:16:53 2021 +0000
@@ -5,6 +5,7 @@
  */ 
 
 #include "MX12.h"
+#include "math.h"
 
 MX12::MX12(PinName tx, PinName rx, int baud)
     : _mx12(tx, rx)    // initializes UnbufferedSerial object
@@ -356,3 +357,59 @@
     return _angle[mot_id];
 }
 */
+
+void MX12::cmd_moteur(float Vavance, float Vlat, float Wz){
+    float coeff=578.7/1023;
+    float W1;
+    float W2;
+    float W3;
+    float Rc;
+    float R;
+    float x1;
+    float x2;
+    float x3;
+    float y1;
+    float y2;
+    float y3;
+    float a1;
+    float a2;
+    float a3;
+    Rc=0.08;  // rayon du chassis*10
+    R=0.019;  // rayon de la roue*10
+    W1=0;
+    W2=0;
+    W3=0;
+    a1 = 0 ;
+    a2 = 2*1.047197551;
+    a3 = -2*1.047197551;
+    x1 = (Rc - 0.007)*cosf(a1);
+    x2 = (Rc - 0.007)*cosf(a2);
+    x3 = (Rc - 0.007)*cosf(a3);
+    y1 = (Rc - 0.007)*sinf(a1);
+    y2 = (Rc - 0.007)*sinf(a2);
+    y3 = (Rc - 0.007)*sinf(a3);
+    //Vtm_smax=0.8; //sert pour calculer valeurs -999->999
+    //Vnm_smax=0.9;
+    //Wcrd_smax=2.9;
+    //if (Vtm_s>Vtm_smax)
+       // {Vtm_s=Vtm_smax;}
+    //if (Vnm_s>Vnm_smax)
+      //  {Vnm_s=Vnm_smax;}
+    //if (Wcrd_s>Wcrd_smax)
+      //  {Wcrd_s=Wcrd_smax;}
+    //if (Wcrd_s<-Wcrd_smax)
+      //  {Wcrd_s=-Wcrd_smax;}
+ 
+    W1=1/Rc*(sinf(a1)*Vavance- sinf(a1)*y1*Wz - cosf(a1)*Vlat + cosf(a1)*x1*Wz); //loi de commande moteur 1
+    W2=1/Rc*(sinf(a2)*Vavance- sinf(a2)*y2*Wz - cosf(a2)*Vlat + cosf(a2)*x2*Wz);
+    W3=1/Rc*(sinf(a3)*Vavance- sinf(a3)*y3*Wz - cosf(a3)*Vlat + cosf(a3)*x3*Wz);
+    printf("%d %d %dn\r",(int)(1000*W1),(int)(1000*W2),(int)(1000*W3));
+  
+    
+    SetSpeed(1,W1);  // impose la vitesse au moteur 1
+    SetSpeed(2,W2);
+    SetSpeed(3,W3);
+ 
+
+    
+    }