![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
for learning
Diff: CAN/function_CAN.cpp
- Revision:
- 57:f4819de54e7a
- Parent:
- 56:6f50d9d3bfee
- Child:
- 60:64181f1d3e60
--- a/CAN/function_CAN.cpp Tue Mar 03 12:27:53 2020 +0000 +++ b/CAN/function_CAN.cpp Fri Mar 06 12:14:38 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; } @@ -285,10 +283,6 @@ VALVE_DEADZONE_MINUS = (int16_t) (msg.data[5] | msg.data[6] << 8); ROM_RESET_DATA(); - - //spi_eeprom_write(RID_VALVE_CNETER, (int16_t) (VALVE_CENTER)); - //spi_eeprom_write(RID_VALVE_DEADZONE_PLUS, (int16_t) (VALVE_DEADZONE_PLUS)); - //spi_eeprom_write(RID_VALVE_DEADZONE_MINUS, (int16_t) (VALVE_DEADZONE_MINUS)); break; } @@ -385,6 +379,7 @@ PISTON_AREA_A = (int16_t) (msg.data[1] | msg.data[2] << 8); PISTON_AREA_B = (int16_t) (msg.data[3] | msg.data[4] << 8); PISTON_AREA_alpha = (double)PISTON_AREA_B/(double)PISTON_AREA_A; + alpha3 = PISTON_AREA_alpha * PISTON_AREA_alpha*PISTON_AREA_alpha; ROM_RESET_DATA(); @@ -449,11 +444,9 @@ } case CRX_SET_VALVE_LIMIT: { - VALVE_LIMIT_MINUS = (int16_t) (msg.data[1] | msg.data[2] << 8); - VALVE_LIMIT_PLUS = (int16_t) (msg.data[3] | msg.data[4] << 8); + VALVE_MIN_POS = (int16_t) (msg.data[1] | msg.data[2] << 8); + VALVE_MAX_POS = (int16_t) (msg.data[3] | msg.data[4] << 8); ROM_RESET_DATA(); - //spi_eeprom_write(RID_VALVE_LIMIT_MINUS, (int16_t) VALVE_LIMIT_MINUS); - //spi_eeprom_write(RID_VALVE_LIMIT_PLUS, (int16_t) VALVE_LIMIT_PLUS); break; } @@ -635,9 +628,8 @@ break; } case CRX_SET_VALVE_CENTER_OFFSET: { - VALVE_CENTER_OFFSET_times10 = (int16_t) (msg.data[1] | msg.data[2] << 8); - VALVE_CENTER_OFFSET = ((float) VALVE_CENTER_OFFSET_times10) * 0.1f; - DDV_CENTER = DDV_CENTER + VALVE_CENTER_OFFSET; + VALVE_CENTER_OFFSET = ((float) ((int16_t) (msg.data[1] | msg.data[2] << 8))) * 0.1f; + VALVE_CENTER = VALVE_CENTER + VALVE_CENTER_OFFSET; ROM_RESET_DATA(); break; } @@ -690,7 +682,7 @@ break; } case CRX_ASK_VALVE_MAX_MIN_POS: { - CAN_TX_DDV_VALVE_DEADZONE(); + //CAN_TX_DDV_VALVE_DEADZONE(); CAN_TX_DDV_VALVE_MAX_MIN_POS(); break; } @@ -712,22 +704,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)>>1) == 0) { //Moog Valve + valve_pos.ref = (double) temp_ref_valve_pos; + } else if (((OPERATING_MODE&0b110)>>1) == 1) { //KNR Valve + valve_pos.ref = (double) temp_ref_valve_pos; + } else { //SW Valve + if(temp_ref_valve_pos >= 0) { + valve_pos.ref = (double)VALVE_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_CENTER)/10000.0f; + } else { + valve_pos.ref = (double)VALVE_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_CENTER)/10000.0f; + } } } else if(address==CID_RX_REF_PWM){ int temp_ref_pwm = (int16_t) (msg.data[0] | msg.data[1] << 8); @@ -749,7 +756,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); @@ -798,7 +805,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); } @@ -914,8 +921,8 @@ temp_msg.id = CID_TX_INFO; temp_msg.len = 7; temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_DEADZONE; - temp_msg.data[1] = (uint8_t) (int) (VALVE_CENTER); - temp_msg.data[2] = (uint8_t) ((int) (VALVE_CENTER) >> 8); + temp_msg.data[1] = (uint8_t) VALVE_CENTER; + temp_msg.data[2] = (uint8_t) (VALVE_CENTER >> 8); temp_msg.data[3] = (uint8_t) (int) (VALVE_DEADZONE_PLUS); temp_msg.data[4] = (uint8_t) ((int) (VALVE_DEADZONE_PLUS) >> 8); temp_msg.data[5] = (uint8_t) (int) (VALVE_DEADZONE_MINUS); @@ -1046,10 +1053,10 @@ temp_msg.id = CID_TX_INFO; temp_msg.len = 5; temp_msg.data[0] = (uint8_t) CTX_SEND_VALVE_LIMIT; - temp_msg.data[1] = (uint8_t) VALVE_LIMIT_MINUS; - temp_msg.data[2] = (uint8_t) (VALVE_LIMIT_MINUS >> 8); - temp_msg.data[3] = (uint8_t) VALVE_LIMIT_PLUS; - temp_msg.data[4] = (uint8_t) (VALVE_LIMIT_PLUS >> 8); + temp_msg.data[1] = (uint8_t) VALVE_MIN_POS; + temp_msg.data[2] = (uint8_t) (VALVE_MIN_POS >> 8); + temp_msg.data[3] = (uint8_t) VALVE_MAX_POS; + temp_msg.data[4] = (uint8_t) (VALVE_MAX_POS >> 8); can.write(temp_msg); } @@ -1129,9 +1136,11 @@ float temp_valve_deadzone_plus = 0.0f; float temp_ddv_center = 0.0f; - temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)DDV_CENTER)/((double)VALVE_MAX_POS-(double)DDV_CENTER); - temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER); - temp_ddv_center = 0.0f; + //temp_valve_deadzone_plus = 10000.0f*((double)VALVE_DEADZONE_PLUS-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER); + //temp_valve_deadzone_minus = -10000.0f*((double)VALVE_DEADZONE_MINUS-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER); + temp_valve_deadzone_plus = (double)VALVE_DEADZONE_PLUS; + temp_valve_deadzone_minus = (double)VALVE_DEADZONE_MINUS; + temp_ddv_center = (double)VALVE_CENTER; temp_msg.id = CID_TX_INFO; temp_msg.len = 7; @@ -1209,10 +1218,10 @@ int16_t valve_pos_vs_pwm; // 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); + if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_CENTER) { + valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_CENTER); } else { - valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)DDV_CENTER)/((double)VALVE_MIN_POS-(double)DDV_CENTER); + valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER); } int16_t PWM_VALVE_ID; @@ -1234,12 +1243,12 @@ int32_t valve_pos_vs_flowrate; 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 VALVE_POS_VALVE_ID = ID_index_array[canindex] * 10 + VALVE_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)); + if(VALVE_POS_VALVE_ID>=VALVE_CENTER) { + temp_valve_pos = (int16_t) (10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_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_valve_pos = (int16_t) (-10000.0f*((double)VALVE_POS_VALVE_ID-(double)VALVE_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_CENTER)); } temp_msg.id = CID_TX_INFO; @@ -1276,9 +1285,9 @@ float temp_valve_min_pos = 0.0f; float temp_ddv_center = 0.0f; - temp_valve_max_pos = 10000.0f; - temp_valve_min_pos = -10000.0f; - temp_ddv_center = 0.0f; + temp_valve_max_pos = VALVE_MAX_POS; + temp_valve_min_pos = VALVE_MIN_POS; + temp_ddv_center = VALVE_CENTER; temp_msg.id = CID_TX_INFO; temp_msg.len = 7; @@ -1345,7 +1354,7 @@ CANMessage temp_msg; temp_msg.id = CID_TX_PRES; - temp_msg.len = 8; + temp_msg.len = 4; temp_msg.data[0] = (uint8_t) t_pres_a; temp_msg.data[1] = (uint8_t) (t_pres_a >> 8); temp_msg.data[2] = (uint8_t) t_pres_b; @@ -1357,8 +1366,8 @@ void CAN_TX_PWM(int16_t t_pwm) { CANMessage temp_msg; - temp_msg.id = CID_TX_POSITION; - temp_msg.len = 8; + temp_msg.id = CID_TX_VOUT; + temp_msg.len = 2; temp_msg.data[0] = (uint8_t) t_pwm; temp_msg.data[1] = (uint8_t) (t_pwm >> 8); @@ -1389,3 +1398,5 @@ + +