Diff: MX.cpp

Revision:

19:16d5f8a3311f

Parent:

17:6f85605f793d

Child:

20:029e0fe30b0a

--- a/MX.cpp	Fri Jul 26 12:40:22 2019 +0000
+++ b/MX.cpp	Tue Oct 22 12:39:36 2019 +0000
@@ -102,7 +102,7 @@
          goal[i] = (degrees[i]) * (float)(4095/360);        
     }
      
-    for (int i=0 ; i<_Nmotor ; i++){                                      //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8)
+    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
             }
@@ -294,6 +294,28 @@
 }
 
 
+int MX::SyncGoalPWM(float values[], int ID) {
+    
+    uint8_t byte = 2;     //2 is dimension in bytes of instruction 
+    char data[byte*_Nmotor];           
+    int goal[_Nmotor];
+                
+    for (int i=0 ; i<_Nmotor ; i++){                                      //set goal array with goal in RAW(uint32) values from DEGREES(float)
+         goal[i] = (values[i]) * (float)(885/100);        
+    }
+     
+    for (int i=0 ; i<_Nmotor ; i++){                                      //set data array in 2 bytes sequence (split RAW(uint32) in 2*bytes(uint8)
+        for (int j = 0 ; j < byte; j++){
+            data[j+(i*byte)] = (goal[i] >> (j*8) ) & 0xff;    // top 8 bits
+            }
+    }            
+    // write the packet, and return the error code
+    int rVal = SyncSendPacket(MX_REG_GOAL_PWM, byte, data, ID);     
+    
+    return(rVal);
+}
+
+
 float* MX::SyncGetPosition(void) {
     printf("\n SYNC GET POSITION ");
     char* data;