![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Original Version of STM Board
Diff: CAN/function_CAN.cpp
- Revision:
- 54:647072f5307a
- Parent:
- 52:8ea76864368a
- Child:
- 55:b25725257569
--- a/CAN/function_CAN.cpp Wed Feb 19 05:48:57 2020 +0000 +++ b/CAN/function_CAN.cpp Tue Feb 25 12:56:39 2020 +0000 @@ -102,10 +102,7 @@ case CRX_SET_CAN_FREQ: { CAN_FREQ = (int16_t) (msg.data[1] | msg.data[2] << 8); - ROM_RESET_DATA(); - - //spi_eeprom_write(RID_CAN_FREQ, (int16_t) CAN_FREQ); break; } @@ -116,7 +113,8 @@ } case CRX_SET_CONTROL_MODE: { - CONTROL_MODE = (int16_t) (msg.data[1]); + //CONTROL_MODE = (int16_t) (msg.data[1]); + CONTROL_UTILITY_MODE = (int16_t) (msg.data[1]); if (CONTROL_MODE == 22) { //MODE_FIND_HOME FLAG_FIND_HOME = true; } @@ -230,8 +228,8 @@ } case CRX_SET_HOMEPOS: { - CONTROL_MODE = 22; - SETTING_SWITCH = (int16_t) (msg.data[1]); + //CONTROL_MODE = 22; + CONTROL_UTILITY_MODE = 22; break; } @@ -694,6 +692,10 @@ CAN_TX_DDV_VALVE_MAX_MIN_POS(); break; } + case CRX_DELAY_TEST:{ + flag_delay_test = 1; + break; + } default: break; @@ -708,22 +710,37 @@ if(address==CID_RX_CMD){ 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); + int16_t temp_pos = (int16_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); int16_t temp_vel = (int16_t) (msg.data[2] | msg.data[3] << 8); + int16_t temp_torq = (int16_t) (msg.data[4] | msg.data[5] << 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 * 100.0f; + if((OPERATING_MODE&0b001)==0) { // Rotary Actuator + pos.ref = (double)temp_pos * 1.0f; + vel.ref = (double)temp_vel * 10.0f; + } + else { //Linear Actuator + 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_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; + + if ((OPERATING_MODE&0b110) == 0) { //Moog Valve + valve_pos.ref = (double) temp_ref_valve_pos; + } else if ((OPERATING_MODE&0b110) == 1) { //KNR Valve + valve_pos.ref = (double) temp_ref_valve_pos; + } else { //SW Valve + 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); @@ -745,7 +762,7 @@ temp_msg.data[2] = (uint8_t) CAN_FREQ; temp_msg.data[3] = (uint8_t) (CAN_FREQ >> 8); temp_msg.data[4] = (uint8_t) (flag_err[7] << 7 | flag_err[6] << 6 | flag_err[5] << 5 | flag_err[4] << 4 | flag_err[3] << 3 | flag_err[2] << 2 | flag_err[1] << 1 | flag_err[0]); - temp_msg.data[5] = (uint8_t) CONTROL_MODE; + temp_msg.data[5] = (uint8_t) CONTROL_UTILITY_MODE; temp_msg.data[6] = (uint8_t) OPERATING_MODE; can.write(temp_msg); @@ -794,7 +811,7 @@ temp_msg.id = CID_TX_INFO; temp_msg.len = 2; temp_msg.data[0] = (uint8_t) CTX_SEND_CONTROL_MODE; - temp_msg.data[1] = (uint8_t) CONTROL_MODE; + temp_msg.data[1] = (uint8_t) CONTROL_UTILITY_MODE; can.write(temp_msg); } @@ -1056,7 +1073,7 @@ temp_msg.id = CID_TX_INFO; temp_msg.len = 3; temp_msg.data[0] = (uint8_t) CTX_SEND_ENC_PULSE_PER_POSITION; - int temp_enc_pulse_per_position = (int) (ENC_PULSE_PER_POSITION * 10.0f); + int temp_enc_pulse_per_position = (int) (ENC_PULSE_PER_POSITION); temp_msg.data[1] = (uint8_t) temp_enc_pulse_per_position; temp_msg.data[2] = (uint8_t) (temp_enc_pulse_per_position >> 8); @@ -1384,3 +1401,4 @@ +