![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: CAN/function_CAN.cpp
- Revision:
- 49:e7bcfc244d40
- Parent:
- 48:889798ff9329
- Child:
- 50:3c630b5eba9f
--- a/CAN/function_CAN.cpp Wed Jan 29 13:50:24 2020 +0000 +++ b/CAN/function_CAN.cpp Wed Feb 05 08:35:58 2020 +0000 @@ -8,7 +8,8 @@ // CAN ID Setting Variables int CID_RX_CMD = 100; int CID_RX_REF_POSITION = 200; -int CID_RX_REF_PWM = 300; +int CID_RX_REF_VALVE_POS = 300; +int CID_RX_REF_PWM = 400; int CID_TX_INFO = 1100; int CID_TX_POSITION = 1200; @@ -29,6 +30,7 @@ CID_RX_CMD = (int) (BNO + INIT_CID_RX_CMD); CID_RX_REF_POSITION = (int) (BNO + INIT_CID_RX_REF_POSITION); + CID_RX_REF_VALVE_POS = (int) (BNO + INIT_CID_RX_REF_VALVE_POS); CID_RX_REF_PWM = (int) (BNO + INIT_CID_RX_REF_PWM); CID_TX_INFO = (int) (BNO + INIT_CID_TX_INFO); @@ -322,6 +324,20 @@ break; } + + case CRX_ASK_FLOWFORCE_GAIN: { + CAN_TX_FLOWFORCE_GAIN(); + + break; + } + + case CRX_SET_FLOWFORCE_GAIN: { + FLOWFORCE_GAIN = (int16_t) (msg.data[1] | msg.data[2] << 8); + + ROM_RESET_DATA(); + + break; + } case CRX_ASK_VALVE_FF: { CAN_TX_VALVE_FF(); @@ -716,13 +732,16 @@ pos.ref = (double)temp_pos * 4.0f; vel.ref = (double)temp_vel * 100.0f; torq.ref = (double)temp_torq * 0.1f; - } else if(address==CID_RX_REF_PWM) { + } else if(address==CID_RX_REF_VALVE_POS) { int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8); if(temp_ref_valve_pos >= 0){ valve_pos.ref = (double)DDV_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)DDV_CENTER)/10000.0f ; }else{ valve_pos.ref = (double)DDV_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)DDV_CENTER)/10000.0f; } + } else if(address==CID_RX_REF_PWM){ + int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8); + Vout.ref = (double) temp_ref_pwm; } } @@ -828,7 +847,7 @@ } void CAN_TX_VOLTAGE_SUPPLY(void) { - int16_t send_voltage_supply = (int16_t) (SUPPLY_VOLTAGE * 10); + int16_t send_voltage_supply = (int16_t) (SUPPLY_VOLTAGE * 10.0f); CANMessage temp_msg; @@ -842,7 +861,7 @@ } void CAN_TX_VOLTAGE_VALVE(void) { - int16_t send_voltage_valve = (int16_t) (VALVE_VOLTAGE_LIMIT * 10); + int16_t send_voltage_valve = (int16_t) (VALVE_VOLTAGE_LIMIT * 10.0f); CANMessage temp_msg; @@ -936,6 +955,18 @@ can.write(temp_msg); } +void CAN_TX_FLOWFORCE_GAIN(void) { + CANMessage temp_msg; + + temp_msg.id = CID_TX_INFO; + temp_msg.len = 3; + temp_msg.data[0] = (uint8_t) CTX_SEND_FLOWFORCE_GAIN; + temp_msg.data[1] = (uint8_t) FLOWFORCE_GAIN; + temp_msg.data[2] = (uint8_t) (FLOWFORCE_GAIN >> 8); + + can.write(temp_msg); +} + void CAN_TX_VALVE_FF(void) { CANMessage temp_msg; @@ -1195,7 +1226,13 @@ void CAN_TX_VALVE_PWM_VS_VALVE_POS(int8_t canindex) { CANMessage temp_msg; int16_t valve_pos_vs_pwm; - valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]); +// valve_pos_vs_pwm = (int16_t) (VALVE_POS_VS_PWM[canindex]); + + if(VALVE_POS_VS_PWM[canindex]>=DDV_CENTER) { + valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER); + } else { + valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER); + } int16_t PWM_VALVE_ID; PWM_VALVE_ID = ID_index_array[canindex] * 1000; @@ -1217,16 +1254,22 @@ valve_pos_vs_flowrate = (int32_t) (JOINT_VEL[canindex]); int16_t VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + (int) DDV_CENTER; + int16_t temp_valve_pos = 0; + if(VALVE_POS_VALVE_ID>=DDV_CENTER) { + temp_valve_pos = (int16_t) (10000.0f*((double)VALVE_POS_VALVE_ID-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER)); + } else { + temp_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER)); + } temp_msg.id = CID_TX_INFO; - temp_msg.len = 7; + temp_msg.len = 8; temp_msg.data[0] = (uint8_t) CTX_VALVE_POS_VS_FLOWRATE; - temp_msg.data[1] = (uint8_t) VALVE_POS_VALVE_ID; - temp_msg.data[2] = (uint8_t) (VALVE_POS_VALVE_ID >> 8); - temp_msg.data[3] = (uint8_t) valve_pos_vs_flowrate; - temp_msg.data[4] = (uint8_t) (valve_pos_vs_flowrate >> 8); - temp_msg.data[5] = (uint8_t) (valve_pos_vs_flowrate >> 16); - temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 24); + temp_msg.data[1] = (uint8_t) temp_valve_pos; + temp_msg.data[2] = (uint8_t) (temp_valve_pos >> 8); + temp_msg.data[5] = (uint8_t) valve_pos_vs_flowrate; + temp_msg.data[6] = (uint8_t) (valve_pos_vs_flowrate >> 8); + temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 16); + temp_msg.data[7] = (uint8_t) (valve_pos_vs_flowrate >> 24); can.write(temp_msg); } @@ -1344,3 +1387,4 @@ +