Diff: MX12.cpp

Revision:

29:0484cbad0770

Parent:

28:c7402e1014b4

Child:

30:4e7cf52ac2dd

--- a/MX12.cpp	Fri Dec 03 08:53:17 2021 +0000
+++ b/MX12.cpp	Wed Jan 12 15:38:34 2022 +0000
@@ -359,9 +359,28 @@
 }
 */
 
+
+
+
+
+
 void MX12::cmd_moteur(float Vavance, float Vlat, float Wz)
 {
-    float coeff=578.7/1023;
+    // mettre en mode wh
+    char data[2];
+    data[1] = 0;
+    data[0] = 0;
+    _bus_state = SerialState::Writing;
+    rw(1, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data);
+    rw(2, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data);
+    rw(3, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data);
+
+
+    _bus_state = SerialState::Writing;
+    rw(1, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data);
+    rw(2, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data);
+    rw(3, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data);
+
     float W1;
     float W2;
     float W3;
@@ -381,15 +400,15 @@
     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);
+    a1 = 4.74 ;
+    a2 = -5.8;
+    a3 = 3.68;
+    x1 = -0.0032;
+    x2 = 0.0616;
+    x3 = -0.0645;
+    y1 = 0.072;
+    y2 = -0.0356;
+    y3 = -0.0376;
     //Vtm_smax=0.8; //sert pour calculer valeurs -999->999
     //Vnm_smax=0.9;
     //Wcrd_smax=2.9;
@@ -402,13 +421,13 @@
     //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);
+    W1=1/R*(-cosf(a1)*Vavance + sinf(a1)*y1*Wz - sinf(a1)*Vlat + cosf(a1)*x1*Wz); //loi de commande moteur 1
+    W2=1/R*(-cosf(a2)*Vavance + sinf(a2)*y2*Wz - sinf(a2)*Vlat + cosf(a2)*x2*Wz);
+    W3=1/R*(-cosf(a3)*Vavance + sinf(a3)*y3*Wz - sinf(a3)*Vlat + cosf(a3)*x3*Wz);
     printf("%d %d %dn\r",(int)(1000*W1),(int)(1000*W2),(int)(1000*W3));
 
 
-    SetSpeed_rad_s(1,-W1);  // impose la vitesse au moteur 1
+    SetSpeed_rad_s(1,W1);  // impose la vitesse au moteur 1
     SetSpeed_rad_s(2,W2);
     SetSpeed_rad_s(3,W3);
 
@@ -416,6 +435,8 @@
 
 }
 
+
+
 void MX12::eteindre_moteurs()
 {
     char data[1];
@@ -432,10 +453,64 @@
     rw(3, CONTROL_TABLE_TORQUE_ENABLE, 1, data);
 }
 
-void MX12::cmd_moteur_position(int id, float angle)
+
+
+void MX12::cmd_moteur_multiturn(float pos1, float pos2, float pos3)
 {
-    char data[1];
-    data[0] = nbTours;
+    // eteindre les moteurs
+    eteindre_moteurs();
+    // mettre en mode multiturn
+    char data[2];
+    data[1] = 255;
+    data[0] = 15;
+    _bus_state = SerialState::Writing;
+    rw(1, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data);
+    rw(2, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data);
+    rw(3, CONTROL_TABLE_CW_ANGLE_LIMIT, 2, data);
+
+
     _bus_state = SerialState::Writing;
-    rw(id, CONTROL_TABLE_GOAL_POSITION, 2, data);
+    rw(1, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data);
+    rw(2, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data);
+    rw(3, CONTROL_TABLE_CCW_ANGLE_LIMIT, 2, data);
+
+    //relever la position
+    int pas1 = pos1-28672/60;
+    int pas2 = pos2-28672/60;
+    int pas3 = pos3-28672/60;
+
+    for (int i = 0; i < 61; i++) {
+        int goal_position1 = i * pas1 - 28672;
+        if (goal_position1 < 0) {
+            goal_position1 = goal_position1 + 28627 + 36864;
+        }
+        char data1[2];
+        data1[1] = goal_position1%256;
+        data1[0] = goal_position1/256;
+
+        int goal_position2 = i * pas2 - 28672;
+        if (goal_position2 < 0) {
+            goal_position2 = goal_position2 + 28627 + 36864;
+        }
+        char data2[2];
+        data2[1] = goal_position2%256;
+        data2[0] = goal_position2/256;
+
+        int goal_position3 = i * pas3 - 28672;
+        if (goal_position3 < 0) {
+            goal_position3 = goal_position3 + 28627 + 36864;
+        }
+        char data3[2];
+        data3[1] = goal_position3%256;
+        data3[0] = goal_position3/256;
+
+        rw(1, CONTROL_TABLE_GOAL_POSITION, 2, data1);
+        thread_sleep_for(60);
+        rw(2, CONTROL_TABLE_GOAL_POSITION, 2, data2);
+        thread_sleep_for(60);
+        rw(3, CONTROL_TABLE_GOAL_POSITION, 2, data3);
+        thread_sleep_for(60);
+
+        thread_sleep_for(1000);
+    }
 }