Diff: RoboteqController.cpp

Revision:

4:1ee0a235c997

Parent:

3:a03c91082856

Child:

6:f9c4795448c1

--- a/RoboteqController.cpp	Fri May 18 09:32:45 2018 +0000
+++ b/RoboteqController.cpp	Tue May 29 15:41:49 2018 +0000
@@ -32,7 +32,7 @@
     if(Id == Id_CSS_ANS){
         CANMsg2SDO(msg, SDOmsg);
         
-        switch(SDOmsg.index){
+        switch(SDOmsg.idx){
             case qBS: // Brushless speed in rpm
                 speed = (int16_t) (SDOmsg.data[1] << 8) + SDOmsg.data[0];
             break;
@@ -71,25 +71,25 @@
 }
 
 void RoboteqController::SDO_command(uint8_t n, uint16_t idx, uint8_t sub_idx, uint8_t *data){
-    char buffer[8];
+    char msg[8];
     
-    buffer[0] = (CSS_CMD << 4) + (n << 2);
-    buffer[1] = idx;
-    buffer[2] = (idx >> 8);
-    buffer[3] = sub_idx;
+    msg[0] = (CSS_CMD << 4) + (n << 2);
+    msg[1] = idx;
+    msg[2] = (idx >> 8);
+    msg[3] = sub_idx;
     
     // Load bytes 4-7 from data array
     for(int i = 0 ; i < 4-n ; i++){
-        buffer[4+i] = *(data+i);
+        msg[4+i] = *(data+i);
     }
     
-    writeOnCAN(Id_CSS_REQ, buffer, 8);
+    writeOnCAN(Id_CSS_REQ, msg, 8);
 }
 
 void RoboteqController::CANMsg2SDO(const CANMessage& CANmsg, SDOMessage& SDOmsg){
     SDOmsg.css = (CANmsg.data[0] & 0xF0 >> 4);
     SDOmsg.n = (CANmsg.data[0] & 0x0F >> 2);
-    SDOmsg.index = CANmsg.data[1] + (CANmsg.data[2] << 8);
+    SDOmsg.idx = CANmsg.data[1] + (CANmsg.data[2] << 8);
     SDOmsg.subindex = CANmsg.data[3];
     SDOmsg.data[0] = CANmsg.data[4];
     SDOmsg.data[1] = CANmsg.data[5];
@@ -113,9 +113,9 @@
     writeOnCAN(RPDO_ID[n-1] + node_id, buffer, 8);
 }
 
-void RoboteqController::TPDO_parse(const CANMessage& CANmsg, int32_t &user_var1, int32_t &user_var2){
-    user_var1 = CANmsg.data[0] + (CANmsg.data[1] << 8) + (CANmsg.data[2] << 16) + (CANmsg.data[3] << 24);
-    user_var2 = CANmsg.data[4] + (CANmsg.data[5] << 8) + (CANmsg.data[6] << 16) + (CANmsg.data[7] << 24);
+void RoboteqController::TPDO_parse(const CANMessage& CANmsg, int32_t* p_user_var1, int32_t* p_user_var2){
+    *p_user_var1 = CANmsg.data[0] + (CANmsg.data[1] << 8) + (CANmsg.data[2] << 16) + (CANmsg.data[3] << 24);
+    *p_user_var2 = CANmsg.data[4] + (CANmsg.data[5] << 8) + (CANmsg.data[6] << 16) + (CANmsg.data[7] << 24);
 }
 
 void RoboteqController::TPDO_receive(uint8_t n, const CANMessage& CANmsg){
@@ -134,3 +134,15 @@
     }
     */
 }
+
+
+void RoboteqController::sendCommand(uint32_t cmd){
+    uint8_t data[4];
+    
+    data[0] = cmd;
+    data[1] = (cmd>>8);
+    data[2] = (cmd>>16);
+    data[3] = (cmd>>24);
+    
+    SDO_command(0, cCANGO, mot_num, data);
+}