Revision 22:5d3f37356915, committed 2019-11-05

Comitter:

marcodesilva

Date:

Tue Nov 05 14:26:34 2019 +0000

Parent:

21:fe5dd48bebc6

Commit message:

con Corrente e Settings Profili di Acc e Vel

Changed in this revision

--- a/MX.cpp	Tue Nov 05 08:57:38 2019 +0000
+++ b/MX.cpp	Tue Nov 05 14:26:34 2019 +0000
@@ -320,6 +320,84 @@
 }
 
 
+
+int MX::SyncProfileAccel(float profileValAcc[]) {
+    
+    char data[4*_Nmotor];   //4 is dimension in bytes of instruction         
+    int goal[_Nmotor];
+                
+    for (int i=0 ; i<_Nmotor ; i++){                                      //set goal array with goal in RAW(uint32) values from DEGREES(float)
+        goal[i] = (profileValAcc[i]);      
+    }
+     
+    for (int i=0 ; i<_Nmotor ; i++){                                      //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 0-255
+        for (int j = 0 ; j < 4; j++){
+            data[j+(i*4)] = (goal[i] >> (j*8) ) & 0xff;                   // top 8 bits
+            }
+    }      
+      
+    #if SYNC_SETGOAL_DEBUG    
+    printf("\n##########################################");      
+    printf("\n          SYNC SET PROFILE ACCELERATION \n ");
+    printf("\nACCELLERATION CHOSEN FOR EACH MOTOR\n");
+    printf("Acceleration[ID]: value(0-32767) \n");
+    for (int i=0; i<_Nmotor ; i++){
+        printf("goal[%d] :   %d        ",_broadcastID[i],goal[i]);
+    }
+    printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n");
+    for (int i=0 ; i<_Nmotor ; i++){                                      //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8)
+        for (int j = 0 ; j < 4; j++){
+            printf("data[%d]: %02x\n",j+(i*4),data[j+(i*4)]);             //debug data 
+        }
+        printf("\n");           
+    }    
+    #endif   
+        
+    // write the packet, and return the error code
+    int rVal = SyncSendPacket( MX_REG_PROFILE_ACCELER , 4, data);
+    
+    return(rVal);
+}
+
+
+int MX::SyncProfileVel(float profileValueVel[]) {
+    
+    char data[4*_Nmotor];   //4 is dimension in bytes of instruction         
+    int goal[_Nmotor];
+                
+    for (int i=0 ; i<_Nmotor ; i++){                                      //set goal array with goal in RAW(uint32) values from DEGREES(float)
+        goal[i] = (profileValueVel[i]);      
+    }
+     
+    for (int i=0 ; i<_Nmotor ; i++){                                      //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 0-255
+        for (int j = 0 ; j < 4; j++){
+            data[j+(i*4)] = (goal[i] >> (j*8) ) & 0xff;                   // top 8 bits
+            }
+    }      
+      
+    #if SYNC_SETGOAL_DEBUG    
+    printf("\n##########################################");      
+    printf("\n          SYNC SET PROFILE VELOCITY \n ");
+    printf("\nVELOCITY CHOSEN FOR EACH MOTOR\n");
+    printf("Velocity[ID]: value(0-44) \n");
+    for (int i=0; i<_Nmotor ; i++){
+        printf("goal[%d] :   %d        ",_broadcastID[i],goal[i]);
+    }
+    printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n");
+    for (int i=0 ; i<_Nmotor ; i++){                                      //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8)
+        for (int j = 0 ; j < 4; j++){
+            printf("data[%d]: %02x\n",j+(i*4),data[j+(i*4)]);             //debug data 
+        }
+        printf("\n");           
+    }    
+    #endif   
+        
+    // write the packet, and return the error code
+    int rVal = SyncSendPacket( MX_REG_PROFILE_VELOCITY , 4, data);
+    
+    return(rVal);
+}
+
 void MX::SyncGetPosition(float* angle) {
     int bytes = 4;
     int NumberOfMotor  = _Nmotor;
@@ -327,7 +405,6 @@
     char data[(11+bytes)*NumberOfMotor];
     
     int32_t position[NumberOfMotor];
-    int IDreceived[NumberOfMotor];
     float ScaleFactor = (float)360/4095;  
 
 
@@ -335,23 +412,21 @@
 
     
     for (int i=0 ;i<_Nmotor ;i++){  
-    
-        IDreceived[i] = (int)data[4 +(11+bytes)*i];    
-         
         position[i]   = (int)data[12+(11+bytes)*i] << 24;         
         position[i]  |= (int)data[11+(11+bytes)*i] << 16;
         position[i]  |= (int)data[10+(11+bytes)*i] << 8;
         position[i]  |= (int)data[9 +(11+bytes)*i];
     }   
+    
     for (int i=0 ;i<_Nmotor ;i++){
-    // angle(degree) obtained from position(0 - 4095)
-    position[i] = position[i]-2048;
-    angle[i] = (float)position[i]*ScaleFactor;
+        // angle(degree) obtained from position(0 - 4095)
+        position[i] = position[i]-2048;
+        angle[i] = (float)position[i]*ScaleFactor;
     }
 
 #if SYNC_GET_POSITION_DEBUG    
     for (int i=0 ;i<_Nmotor ;i++){     
-        printf("\nGet RAW position data from ID: %d\n",IDreceived[i]);    
+        printf("\nGet RAW current data from ID: %d\n",_broadcastID[i]);  
         printf("  Data[%d]: 0x%02x\n",(9 +(11+bytes)*i),data[9 +(11+bytes)*i]);
         printf("  Data[%d]: 0x%02x\n",(10 +(11+bytes)*i),data[10 +(11+bytes)*i]);
         printf("  Data[%d]: 0x%02x\n",(11 +(11+bytes)*i),data[11 +(11+bytes)*i]);
@@ -366,43 +441,46 @@
 
 void MX::SyncGetCurrent(float* presentCurrent) {
 
-    int bytes = 2;
+    printf("\n##########################################");
+    printf("\n SYNC GET CURRENT ");
+    
+    int bytes = 4;
     char data[(11+bytes)*_Nmotor];
+    int32_t current[bytes];
 
-    int32_t current[bytes];
-    int NumberOfMotor  = sizeof(_Nmotor);
-    int IDreceived[NumberOfMotor];
     //float presentCurrent[NumberOfMotor] ;
     float scaleFactor = (float)3.36;  //  3.36mA is a unit for scale from 0 to 1941 in DEC 
 
      
     //char Status[(11+bytes)*_Nmotor]; (11+bytes)
     SyncReadPacket(MX_REG_PRESENT_CURRENT, bytes, data);
+
     for (int i=0 ;i<_Nmotor ;i++){  
-    
-        IDreceived[i] = (int)data[4 +(11+bytes)*i];    
-         
-        current[i]   = (int)data[10+(11+bytes)*i] << 8;
+        current[i]   = (int)data[12+(11+bytes)*i] << 24;         
+        current[i]  |= (int)data[11+(11+bytes)*i] << 16;
+        current[i]  |= (int)data[10+(11+bytes)*i] << 8;
         current[i]  |= (int)data[9 +(11+bytes)*i];
-        
-        
-        
     }   
+
     for (int i=0 ;i<_Nmotor ;i++){
+        if ((int)data[10+(11+bytes)*i] == 255){
+            current[i] = current[i] - (float)65535;
+        }
     // PresentCurrent (mA) obtained from current (0 - 1941)
     presentCurrent[i] = (float)current[i]*scaleFactor;
     }
 
 #if SYNC_GET_CURRENT_DEBUG    
     for (int i=0 ;i<_Nmotor ;i++){     
-        printf("\nGet RAW current data from ID: %d\n",IDreceived[i]);    
+        printf("\nGet RAW current data from ID: %d\n",_broadcastID[i]);    
         printf("  Data[%d]: 0x%02x\n",(9 +(11+bytes)*i),data[9 +(11+bytes)*i]);
         printf("  Data[%d]: 0x%02x\n",(10 +(11+bytes)*i),data[10 +(11+bytes)*i]);
+        printf("  Data[%d]: 0x%02x\n",(11 +(11+bytes)*i),data[11 +(11+bytes)*i]);
+        printf("  Data[%d]: 0x%02x\n",(12 +(11+bytes)*i),data[12 +(11+bytes)*i]);
         printf("Converted Current (0 - 1941): %d\n",current[i]);
-        printf("Converted Present Current %f\n\n",presentCurrent[i]); 
+        printf("Converted Present Current %f mA\n\n",presentCurrent[i]);
     }           
 #endif      
-
 }
 
 
@@ -470,11 +548,11 @@
             #endif
             TxBuf[t] = ID;
         }else{
+            #if SYNC_SENDPACKET_DEBUG
+            printf("\n\nMULTI MOTOR "); 
+            printf("\nID: %d", _broadcastID[i]);
+            #endif
             TxBuf[t] = _broadcastID[i];
-            #if SYNC_SENDPACKET_DEBUG
-            printf("\n\nMULTI MOTOR"); 
-            printf("\nbroadcastID: %d", _broadcastID[i]);
-            #endif
         }
         
        for (int j = 0; j < bytes ; j++){           

--- a/MX.h	Tue Nov 05 08:57:38 2019 +0000
+++ b/MX.h	Tue Nov 05 14:26:34 2019 +0000
@@ -42,16 +42,16 @@
 //#define OPERATING_MODE_DEBUG 1
 //#define SETBAUD_DEBUG 1
 
-//#define SYNC_TORQUE_ENABLE_DEBUG 1
-//#define SYNC_SETGOAL_DEBUG 1
+#define SYNC_TORQUE_ENABLE_DEBUG 0
+#define SYNC_SETGOAL_DEBUG 0
 #define SYNC_GET_POSITION_DEBUG 0
-//#define SYNC_SENDPACKET_DEBUG 1
-//#define SYNC_SENDPACKET_DEBUG_PACKETONLY 1
-//#define SYNC_READPACKET_DEBUG_PACKETONLY 0
-//#define SYNC_SET_BAUD_DEBUG 1
-//#define SYNC_GET_CURRENT_DEBUG 0
-//#define SYNC_GOAL_CURRENT_DEBUG 1  
-//#define SYNC_OPERATING_MODE_DEBUG 1
+#define SYNC_SENDPACKET_DEBUG 0
+#define SYNC_SENDPACKET_DEBUG_PACKETONLY 0
+#define SYNC_READPACKET_DEBUG_PACKETONLY 0
+#define SYNC_SET_BAUD_DEBUG 0
+#define SYNC_GET_CURRENT_DEBUG 0
+#define SYNC_GOAL_CURRENT_DEBUG 0  
+#define SYNC_OPERATING_MODE_DEBUG 0
 
   // Protocol 2.0 
 // N.B. all Register values are in HEX, on the on-line manual all are in DEC
@@ -71,6 +71,8 @@
 #define MX_REG_GOAL_CURRENT 0x66
 #define MX_REG_CURRENT_LIMIT 0x26
 #define MX_REG_GOAL_PWM 0x64
+#define MX_REG_PROFILE_ACCELER 0x6C
+#define MX_REG_PROFILE_VELOCITY 0x70
 #define MX_OFF 0
 #define MX_ON 1
 
@@ -214,9 +216,11 @@
     int SyncGoalCurrent(float mAmpere[]);
     int SyncCurrentLimit(float mAmpere, int ID);
     int SyncCurrentLimit(float mAmpere[]);
-    int SyncOperatingMode(int mode[], int ID);
-    int SyncSetBaud(int MotorBaud[], int ID);
-    int SyncGoalPWM(float values[], int ID);
+    int SyncOperatingMode(int mode[], int ID =-1);
+    int SyncSetBaud(int MotorBaud[], int ID =-1 );
+    int SyncGoalPWM(float values[], int ID =-1 );
+    int SyncProfileAccel(float profileValAcc[]);
+    int SyncProfileVel(float profileValueVel[]);
        
 private :