Diff: CAN/function_CAN.cpp

Revision:

41:abbd4e2af68b

Parent:

38:118df027d851

Child:

42:1cf66990ccab

--- a/CAN/function_CAN.cpp	Thu Dec 19 11:39:31 2019 +0000
+++ b/CAN/function_CAN.cpp	Mon Dec 23 04:05:31 2019 +0000
@@ -715,10 +715,15 @@
         unsigned int CMD = msg.data[0];
         ReadCMD(CMD);
     } else if(address==CID_RX_REF_POSITION) {
-        int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24);
-        int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
-        pos.ref = (double)temp_pos;
-        vel.ref = (double)temp_vel;
+        //int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8 | msg.data[2] << 16 | msg.data[3] << 24);
+        int32_t temp_pos = (int32_t) (msg.data[0] | msg.data[1] << 8);
+        //int32_t temp_vel = (int32_t) (msg.data[4] | msg.data[5] << 8 | msg.data[6] << 16 | msg.data[7] << 24);
+        int32_t temp_vel = (int32_t) (msg.data[2] | msg.data[3] << 8);
+        
+        int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 8);
+        pos.ref = (double)temp_pos * 4.0f;
+        vel.ref = (double)temp_vel * 400.0f;
+        torq.ref = (double)temp_torq;
     } else if(address==CID_RX_REF_TORQUE) {
         int16_t temp_torq = (int16_t) (msg.data[0] | msg.data[1] << 8);
         torq.ref = (double)temp_torq;
@@ -1265,19 +1270,17 @@
  Sensor & State Transmission Functions
 *******************************************************************************/
 
-void CAN_TX_POSITION(int32_t t_pos, int32_t t_vel) {
+void CAN_TX_POSITION(int16_t t_pos, int16_t t_vel, int16_t t_torq) {
     CANMessage temp_msg;
 
     temp_msg.id = CID_TX_POSITION;
     temp_msg.len = 8;
     temp_msg.data[0] = (uint8_t) t_pos;
     temp_msg.data[1] = (uint8_t) (t_pos >> 8);
-    temp_msg.data[2] = (uint8_t) (t_pos >> 16);
-    temp_msg.data[3] = (uint8_t) (t_pos >> 24);
-    temp_msg.data[4] = (uint8_t) t_vel;
-    temp_msg.data[5] = (uint8_t) (t_vel >> 8);
-    temp_msg.data[6] = (uint8_t) (t_vel >> 16);
-    temp_msg.data[7] = (uint8_t) (t_vel >> 24);
+    temp_msg.data[2] = (uint8_t) t_vel;
+    temp_msg.data[3] = (uint8_t) (t_vel >> 8);
+    temp_msg.data[4] = (uint8_t) t_torq;
+    temp_msg.data[5] = (uint8_t) (t_torq >> 8);
 
     can.write(temp_msg);
 }