Diff: moteur.cpp

Revision:

0:7bc294a0862d

Child:

1:a0502fd47e2a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/moteur.cpp	Fri Mar 12 10:11:08 2021 +0000
@@ -0,0 +1,46 @@
+#include "MX12.h"
+//extern UnbufferedSerial pc_serie(USBTX,USBRX,115200);
+void cmd_moteur(float Vtm_s, float Vnm_s, float Wcrd_s){
+    static MX12  servo(PC_4,PC_5,115200);
+    float coeff=578.7/1023;
+    float W1;
+    float W2;
+    float W3;
+    float Rc;
+    float R;
+    float Vtc;
+    float Vnc;
+    float Wcc;
+    float Vtm_smax;
+    float Vnm_smax;
+    float Wcrd_smax;
+    Rc=0.08;  // rayon du chassis*10
+    R=0.019;  // rayon de la roue*10
+    W1=0;
+    W2=0;
+    W3=0;
+    //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=(Wcrd_s*Rc-0.5*Vnm_s+0.866*Vtm_s)*coeff; //loi de commande moteur 1
+    W2=(Wcrd_s*Rc-0.5*Vnm_s-0.866*Vtm_s)*coeff;
+    W3=(Wcrd_s*Rc+Vnm_s)*coeff;
+    printf("%d %d %dn\r",(int)(1000*W1),(int)(1000*W2),(int)(1000*W3));
+  
+    
+    servo.SetSpeed(1,W1);  // impose la vitesse au moteur 1
+    servo.SetSpeed(2,W2);
+    servo.SetSpeed(3,W3);
+ 
+
+    
+    }
\ No newline at end of file