Diff: MX12.cpp

Revision:

28:c7402e1014b4

Parent:

27:06850c65b9c8

Child:

29:0484cbad0770

--- a/MX12.cpp	Fri Nov 26 08:55:31 2021 +0000
+++ b/MX12.cpp	Fri Dec 03 08:53:17 2021 +0000
@@ -1,8 +1,8 @@
-/**  
- * @file MX12.ccp  
- * @brief This file contains all the methods of the MX12 class 
- *        whose prototypes are in the MX12.h header file 
- */ 
+/**
+ * @file MX12.ccp
+ * @brief This file contains all the methods of the MX12 class
+ *        whose prototypes are in the MX12.h header file
+ */
 
 #include "MX12.h"
 #include "math.h"
@@ -18,38 +18,41 @@
         8,                /* bits */
         SerialBase::None, /* parity */
         1                 /* stop bit */
-    ); 
+    );
     // Register a callback to process a Rx (receive) interrupt.
     _mx12.attach(callback(this, &MX12::_Rx_interrupt), SerialBase::RxIrq);
-    
+
     // variable used for message reception
     _status_pck = {.raw = "",
                    .n_byte = 0,
-                   .servo_id = 0, 
+                   .servo_id = 0,
                    .length = 0,
                    .error = 0,
                    .n_param = 0,
-                   .param = "", 
+                   .param = "",
                    .received_checksum = 0,
                    .calculated_checksum = 0,
                    .parsed = false,
-                   .valid = false};
-                   
+                   .valid = false
+                  };
+
     _parser_state = {.expected_field = PacketField::Header1,
-                       .byte_index = 0, 
-                       .param_index = 0};
-                       
+                     .byte_index = 0,
+                     .param_index = 0
+                    };
+
     // Internal defaults states
     _bus_state = SerialState::Idle;
-    
+
 }
 
-void MX12::SetSpeed(unsigned char mot_id, float speed) {
+void MX12::SetSpeed(unsigned char mot_id, float speed)
+{
     char data[2];
-    
+
     // Speed absolute value
     int goal = (0x3ff * abs(speed));
- 
+
     // Spin direction (CW is negative)
     if (speed < 0) {
         goal |= (0x1 << 10);
@@ -57,7 +60,7 @@
 
     data[0] = goal & 0xff;
     data[1] = goal >> 8;
-    
+
     // Send instruction
     _bus_state = SerialState::Writing;
     rw(mot_id, CONTROL_TABLE_MOVING_SPEED, 2, data);
@@ -65,44 +68,43 @@
 
 void MX12::SetSpeed_rad_s(unsigned char mot_id, float speed)
 {
-    if (speed > MX12_ABSOLUTE_MAX_SPEED_RAD_S) 
-    {
+    if (speed > MX12_ABSOLUTE_MAX_SPEED_RAD_S) {
         SetSpeed(mot_id, 1);
-    } 
-    else if (speed < -MX12_ABSOLUTE_MAX_SPEED_RAD_S) 
-    {
+    } else if (speed < -MX12_ABSOLUTE_MAX_SPEED_RAD_S) {
         SetSpeed(mot_id, -1);
-    }
-    else 
-    {
+    } else {
         SetSpeed(mot_id, speed / MX12_ABSOLUTE_MAX_SPEED_RAD_S);
     }
 }
 
-char MX12::IsAvailable(void) {
+char MX12::IsAvailable(void)
+{
     return (_bus_state == SerialState::Idle);
 }
 
-void MX12::rw(unsigned char mot_id, char address, char len, char *data) {
-    
+void MX12::rw(unsigned char mot_id, char address, char len, char *data)
+{
+
     /* Set variables for reception from servovotor */
     _answer  = 0;
     _status_pck = {.raw = "",
                    .n_byte = 0,
-                   .servo_id = 0, 
+                   .servo_id = 0,
                    .length = 0,
                    .error = 0,
                    .n_param = 0,
-                   .param = "", 
+                   .param = "",
                    .received_checksum = 0,
                    .calculated_checksum = 0,
                    .parsed = false,
-                   .valid = false};
+                   .valid = false
+                  };
     _parser_state = {.expected_field = PacketField::Header1,
-                     .byte_index = 0, 
-                     .param_index = 0};
+                     .byte_index = 0,
+                     .param_index = 0
+                    };
 
-    
+
     /* Initialise instruction packet to forge.
      * Instruction Packet is the command data sent to the servomotor.
      *
@@ -120,28 +122,27 @@
      */
     char packet[16];
     unsigned char packet_length;
-    
+
     /* Initialise checksum to calculate
-     * It is used to check if packet is damaged during communication. 
+     * It is used to check if packet is damaged during communication.
      * Status Checksum is calculated according to the following formula:
      *
      * Status Checksum = ~( ID + Length + Error + Parameter1 + … Parameter N )
      */
     char checksum = 0x00;
-        
+
     /* header 1 = 0xFF (dynamixel protocol 1.0) */
     packet[0] = 0xff;
-    
+
     /* header 2 = 0xFF (dynamixel protocol 1.0) */
-    packet[1] = 0xff; 
-    
-     /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
+    packet[1] = 0xff;
+
+    /* packet ID i.e. servomotor id (dynamixel protocol 1.0) */
     packet[2] = mot_id;
     checksum += packet[2];
-    
+
     /* Guess instruction type. NULL for read, not NULL for write */
-    if(data == NULL) // read instruction
-    {
+    if(data == NULL) { // read instruction
         /* byte length of the instruction: parameter and checksum field.   */
         /* for read instruction:   1 INSTR +                               */
         /*       2 PARAM (starting address, length of data) + 1 CHKSUM     */
@@ -151,26 +152,24 @@
         /* set write instruction */
         packet[4] = PROTOCOL_INSTRUCTION_READ;
         checksum += packet[4];
-        
+
         /* Param 1: address to read in the Control Table of RAM Area */
         packet[5] = address;
         checksum += packet[5];
-        
+
         /* Param 2: number of bytes to read in the Control Table of RAM Area */
         packet[6] = len;
         checksum += packet[6];
-        
+
         /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
         packet[7] = ~checksum;
-        
+
         packet_length = 8;
-    }
-    else // write instruction
-    {
+    } else { // write instruction
         /* byte length of the instruction: parameter and checksum field     */
         /* For write instruction:   1 INSTR +                               */
         /*       (1+len)PARAM (starting Address, bytes to write) + 1 CHKSUM */
-        packet[3] = 3 + len; 
+        packet[3] = 3 + len;
         checksum += packet[3];
 
         /* set read instruction */
@@ -180,19 +179,19 @@
         /* Param 1: address to write in the "Control Table of RAM Area" */
         packet[5] = address;
         checksum += packet[5];
-        
+
         /* Param 2 to N: data to write in the Control Table of RAM Area */
         for(char i = 0; i < len; i++) {
             packet[6 + i] = data[i];
             checksum += data[i];
         }
-        
+
         /* Checksum = ~( ID + Length + Instruction + Param1 + … Param N ) */
         packet[6 + len] = ~checksum;
-        
+
         packet_length = 7 + len;
     }
-    
+
     // Send packet
     if(mot_id != 0xFE) {
         for(char i = 0; i < packet_length; i++) {
@@ -202,15 +201,16 @@
 }
 
 // Debug function to print Serial read
-void MX12::PrintSerial() 
+void MX12::PrintSerial()
 {
     for(int i = 0; i < _status_pck.n_byte; i++) {
         printf("%x ", _status_pck.raw[i]);
     }
     printf("\n");
 }
- 
-MX12::Status MX12::GetStatus() {
+
+MX12::Status MX12::GetStatus()
+{
     // Return the corresponding status code
     switch(_status_pck.error) {
         case 0:
@@ -241,39 +241,40 @@
             return Unknown;
     }
 }
- 
-void MX12::_Rx_interrupt() {
+
+void MX12::_Rx_interrupt()
+{
 
     char c;
-    
+
     // Try to read serial
     if(_mx12.read(&c, 1)) {
-        
+
         _status_pck.raw[(_parser_state.byte_index)++] = c;
 
         // State-machine parsing
         switch(_parser_state.expected_field) {
-            
+
             /* c char is interpreted as a Header1 field */
             case PacketField::Header1:
-            
+
                 /* do nothing and set next state to Header2 */
                 _parser_state.expected_field = PacketField::Header2;
                 break;
 
             /* c char is interpreted as a Header2 field */
             case PacketField::Header2:
-            
+
                 /* do nothing and set next state to Id */
                 _parser_state.expected_field = PacketField::Id;
                 break;
-                
+
             /* c char is interpreted as ID field */
             case PacketField::Id:
-            
+
                 /* store ID, update checksum and set next state to Length */
                 _status_pck.servo_id = c;
-                _status_pck.calculated_checksum += c; 
+                _status_pck.calculated_checksum += c;
                 _parser_state.expected_field = PacketField::Length;
                 break;
 
@@ -282,34 +283,34 @@
              * where 2 stands for Length field (1 byte) + Error filed (1 byte)
              */
             case PacketField::Length:
-            
-                /* store number of param into _status_pck.n_param, 
-                 * update calculated_checksum and set next state to Error 
+
+                /* store number of param into _status_pck.n_param,
+                 * update calculated_checksum and set next state to Error
                  */
                 _status_pck.n_param = c - 2;
-                _status_pck.calculated_checksum += c; 
+                _status_pck.calculated_checksum += c;
                 _parser_state.expected_field = PacketField::Error;
                 break;
- 
+
             /* c char is interpreted as error status field */
             case PacketField::Error:
-            
-                /* store error status, update checksum 
-                 * and set next state to Data 
+
+                /* store error status, update checksum
+                 * and set next state to Data
                  */
                 _status_pck.error = c;
-                _status_pck.calculated_checksum += c; 
+                _status_pck.calculated_checksum += c;
                 _parser_state.expected_field = PacketField::Data;
                 break;
- 
+
             /* c char is interpreted as a param field */
             case PacketField::Data:
 
-                /* store current param, increase param_index 
+                /* store current param, increase param_index
                  * and update checksum */
                 _status_pck.param[(_parser_state.param_index)++] = c;
-                _status_pck.received_checksum += c; 
-                
+                _status_pck.received_checksum += c;
+
                 /* increase param index (_parser_state.dataCount)
                  * and test if it is the last param to read
                  */
@@ -319,10 +320,10 @@
                     _parser_state.expected_field = PacketField::Checksum;
                 }
                 break;
-            
+
             /* c char is interpreted as Checksum field */
             case PacketField::Checksum:
-            
+
                 /* store received_checksum, set parsed, store n_byte,
                  * evalutate valid and set next state to Header1 */
                 _status_pck.received_checksum = c;
@@ -330,17 +331,17 @@
                 _status_pck.n_byte = _parser_state.byte_index;
                 _status_pck.valid = (_status_pck.received_checksum == c);
                 _parser_state.expected_field = PacketField::Header1;
-                
+
                 /* set seriel state to Idle */
                 _bus_state = SerialState::Idle;
                 break;
-                
+
             default:
-                
-                /* unexpected case. If it occurs it would be due to a 
+
+                /* unexpected case. If it occurs it would be due to a
                  * code error of this class  */
                 break;
-        }        
+        }
     }
 }
 
@@ -358,7 +359,8 @@
 }
 */
 
-void MX12::cmd_moteur(float Vavance, float Vlat, float Wz){
+void MX12::cmd_moteur(float Vavance, float Vlat, float Wz)
+{
     float coeff=578.7/1023;
     float W1;
     float W2;
@@ -392,39 +394,48 @@
     //Vnm_smax=0.9;
     //Wcrd_smax=2.9;
     //if (Vtm_s>Vtm_smax)
-       // {Vtm_s=Vtm_smax;}
+    // {Vtm_s=Vtm_smax;}
     //if (Vnm_s>Vnm_smax)
-      //  {Vnm_s=Vnm_smax;}
+    //  {Vnm_s=Vnm_smax;}
     //if (Wcrd_s>Wcrd_smax)
-      //  {Wcrd_s=Wcrd_smax;}
+    //  {Wcrd_s=Wcrd_smax;}
     //if (Wcrd_s<-Wcrd_smax)
-      //  {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_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);
- 
+
+
 
-   
-    }
-  
-void MX12::eteindre_moteurs(){
-     char data[1];
+}
+
+void MX12::eteindre_moteurs()
+{
+    char data[1];
 
     data[0] = 0;
 
-    
+
     // Send instruction
     _bus_state = SerialState::Writing;
-    rw(1, CONTROL_TABLE_TORQUE_ENABLE , 1, data);
+    rw(1, CONTROL_TABLE_TORQUE_ENABLE, 1, data);
+    _bus_state = SerialState::Writing;
+    rw(2, CONTROL_TABLE_TORQUE_ENABLE, 1, data);
     _bus_state = SerialState::Writing;
-    rw(2, CONTROL_TABLE_TORQUE_ENABLE , 1, data);
+    rw(3, CONTROL_TABLE_TORQUE_ENABLE, 1, data);
+}
+
+void MX12::cmd_moteur_position(int id, float angle)
+{
+    char data[1];
+    data[0] = nbTours;
     _bus_state = SerialState::Writing;
-    rw(3, CONTROL_TABLE_TORQUE_ENABLE , 1, data);
+    rw(id, CONTROL_TABLE_GOAL_POSITION, 2, data);
 }