Sungwoo Kim
/
HydraulicControlBoard_Learning
for learning
Diff: CAN/function_CAN.cpp
- Revision:
- 67:c2812cf26c38
- Parent:
- 66:a8e6799dbce3
- Child:
- 68:328e1be06f5d
diff -r a8e6799dbce3 -r c2812cf26c38 CAN/function_CAN.cpp --- a/CAN/function_CAN.cpp Tue May 12 06:23:46 2020 +0000 +++ b/CAN/function_CAN.cpp Tue May 12 07:43:44 2020 +0000 @@ -20,13 +20,9 @@ // variables uint8_t can_index = 0; + extern DigitalOut LED; -//extern int num_input; -//extern int num_input_array; -extern float input[]; -extern float x_future[]; - /******************************************************************************* * CAN functions ******************************************************************************/ @@ -266,10 +262,10 @@ ROM_RESET_DATA(); } else if (msg.data[1] == 3) { - K_SPRING = (int16_t) (msg.data[2] | msg.data[3] << 8); - D_DAMPER = (int16_t) (msg.data[4] | msg.data[5] << 8); + K_SPRING = (float) (((float) ((int16_t) (msg.data[2] | msg.data[3] << 8))) * 0.1f); + D_DAMPER = (float) (((float) ((int16_t) (msg.data[4] | msg.data[5] << 8))) * 0.01f); - ROM_RESET_DATA(); +// ROM_RESET_DATA(); //For Real-time changing } break; @@ -694,11 +690,6 @@ flag_delay_test = 1; break; } - case CRX_SET_NN_CONTROL_FLAG: { - NN_Control_Flag = (int16_t) msg.data[1]; - CONTROL_UTILITY_MODE = 1; - break; - } default: break; @@ -725,22 +716,11 @@ vel.ref = (double)temp_vel * 10.0f; } else { //Linear Actuator - pos.ref = (double)temp_pos * 4.0f; - vel.ref = (double)temp_vel * 100.0f; + pos.ref = (double)temp_pos * 10.0f; + vel.ref = (double)temp_vel * 256.0f; } - torq.ref = (double)temp_torq * 0.1f; - - x_future[0] = pos.sen/(float)(ENC_PULSE_PER_POSITION); //mm - for(int i=1;i<num_input_array-1;i++){ - x_future[i] = x_future[i+1]; - } - x_future[num_input_array-1] = pos.ref/(float)(ENC_PULSE_PER_POSITION); //mm - - for(int i=0;i<num_input;i++){ - input[i] = (x_future[2*i+2] - x_future[0])*0.2f+0.5f; - } - + torq.ref = (double)temp_torq * 0.1f / (float)(TORQUE_SENSOR_PULSE_PER_TORQUE); } else if(address==CID_RX_REF_VALVE_POS) { int16_t temp_ref_valve_pos = (int16_t) (msg.data[0] | msg.data[1] << 8); @@ -914,8 +894,8 @@ sendIgain = (int16_t) (I_GAIN_JOINT_TORQUE); sendDgain = (int16_t) (D_GAIN_JOINT_TORQUE); } else if (t_type == 3) { - sendPgain = (int16_t) (K_SPRING); - sendIgain = (int16_t) (D_DAMPER); + sendPgain = (int16_t) (K_SPRING * 10.0f); + sendIgain = (int16_t) (D_DAMPER * 100.0f); } CANMessage temp_msg; @@ -1359,15 +1339,26 @@ can.write(temp_msg); } -void CAN_TX_TORQUE(int16_t t_valve_pos, int16_t t_vout) { +//void CAN_TX_TORQUE(int16_t t_valve_pos, int16_t t_vout) { +// CANMessage temp_msg; +// +// temp_msg.id = CID_TX_TORQUE; +// temp_msg.len = 4; +// temp_msg.data[0] = (uint8_t) t_valve_pos; +// temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8); +// temp_msg.data[2] = (uint8_t) t_vout; +// temp_msg.data[3] = (uint8_t) (t_vout >> 8); +// +// can.write(temp_msg); +//} + +void CAN_TX_TORQUE(int16_t t_valve_pos) { CANMessage temp_msg; temp_msg.id = CID_TX_TORQUE; - temp_msg.len = 4; + temp_msg.len = 2; temp_msg.data[0] = (uint8_t) t_valve_pos; temp_msg.data[1] = (uint8_t) (t_valve_pos >> 8); - temp_msg.data[2] = (uint8_t) t_vout; - temp_msg.data[3] = (uint8_t) (t_vout >> 8); can.write(temp_msg); } @@ -1411,14 +1402,4 @@ can.write(temp_msg); -} - - - - - - - - - - +} \ No newline at end of file