![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
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); }