Sungwoo Kim
/
HydraulicControlBoard_Rainbow_v1_2
rainbow
Revision 248:544011ad9c85, committed 2022-06-24
- Comitter:
- Lightvalve
- Date:
- Fri Jun 24 05:55:29 2022 +0000
- Parent:
- 247:bfdf0f479a38
- Child:
- 249:f69cd0553be0
- Commit message:
- 220624_2
Changed in this revision
--- a/CAN/function_CAN.cpp Fri Jun 24 00:18:20 2022 +0000 +++ b/CAN/function_CAN.cpp Fri Jun 24 05:55:29 2022 +0000 @@ -732,11 +732,11 @@ valve_pos.ref = (double) temp_ref_valve_pos; // Unit : pulse (0~30000) } else { //SW Valve if(temp_ref_valve_pos >= 0) { -// valve_pos.ref = (double)VALVE_ELECTRIC_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; - valve_pos.ref = (double)temp_ref_valve_pos; // -10000~10000 + valve_pos.ref = (double)VALVE_ELECTRIC_CENTER + (double)temp_ref_valve_pos * ((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; +// valve_pos.ref = (double)temp_ref_valve_pos; } else { -// valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; - valve_pos.ref = (double)temp_ref_valve_pos; // -10000~10000 + valve_pos.ref = (double)VALVE_ELECTRIC_CENTER - (double)temp_ref_valve_pos * ((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER)/10000.0f; +// valve_pos.ref = (double)temp_ref_valve_pos; } } } else if(address==CID_RX_REF_PWM) { @@ -1089,8 +1089,8 @@ CANMessage temp_msg; int16_t temp_valve_min_pos = 0; int16_t temp_valve_max_pos = 0; - temp_valve_min_pos = (int16_t) ((float) VALVE_MIN_POS * 0.1f); - temp_valve_max_pos = (int16_t) ((float) VALVE_MAX_POS * 0.1f); + temp_valve_min_pos = (int16_t) VALVE_MIN_POS; + temp_valve_max_pos = (int16_t) VALVE_MAX_POS; temp_msg.id = CID_TX_INFO; temp_msg.len = 5; @@ -1246,9 +1246,9 @@ int16_t valve_pos_vs_pwm; if(VALVE_POS_VS_PWM[canindex]>= (float) VALVE_ELECTRIC_CENTER) { - valve_pos_vs_pwm = 10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MAX_POS-(double)VALVE_ELECTRIC_CENTER); + valve_pos_vs_pwm = (int16_t) (10000.0f*((float)VALVE_POS_VS_PWM[canindex]-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER)); } else { - valve_pos_vs_pwm = -10000.0f*((double)VALVE_POS_VS_PWM[canindex]-(double)VALVE_ELECTRIC_CENTER)/((double)VALVE_MIN_POS-(double)VALVE_ELECTRIC_CENTER); + valve_pos_vs_pwm = (int16_t) (-10000.0f*((float)VALVE_POS_VS_PWM[canindex]-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)); } int16_t PWM_VALVE_ID;
--- a/INIT_HW/INIT_HW.cpp Fri Jun 24 00:18:20 2022 +0000 +++ b/INIT_HW/INIT_HW.cpp Fri Jun 24 05:55:29 2022 +0000 @@ -225,7 +225,7 @@ TIM4->CR1 |= TIM_CR1_UDIS; TIM4->EGR |= TIM_EGR_UG; - TIM4->PSC = 10-1; // 10 prescaler, timer counts up in sync with the peripheral clock + TIM4->PSC = 20-1; // 10 prescaler, timer counts up in sync with the peripheral clock TIM4->ARR = 1; TIM4->CNT = 0; TIM4->SR = 0;
--- a/main.cpp Fri Jun 24 00:18:20 2022 +0000 +++ b/main.cpp Fri Jun 24 05:55:29 2022 +0000 @@ -198,7 +198,6 @@ enc.format(8,0); enc.frequency(5000000); //10M enc_cs = 0; - make_delay(); // spi _ enc @@ -360,21 +359,19 @@ } // Valve feedforward for SW valve -void VALVE_POS_CONTROL_DZ(float REF_VALVE_POS_10000) +void VALVE_POS_CONTROL_DZ(float REF_VALVE_POS) { int i = 0; float valve_pos_ref_dz = 0.0f; - if(REF_VALVE_POS_10000 > 10000.0f) { - REF_VALVE_POS_10000 = 10000.0f; - } else if(REF_VALVE_POS_10000 < -10000.0f) { - REF_VALVE_POS_10000 = -10000.0f; + if(REF_VALVE_POS > VALVE_MAX_POS) { + REF_VALVE_POS = VALVE_MAX_POS; + } else if(REF_VALVE_POS < VALVE_MIN_POS) { + REF_VALVE_POS = VALVE_MIN_POS; } - if (REF_VALVE_POS_10000 >= 0) { -// valve_pos_ref_dz = REF_VALVE_POS_10000/10000.0f * (VALVE_MAX_POS-VALVE_DEADZONE_PLUS) + VALVE_DEADZONE_PLUS; - valve_pos_ref_dz = REF_VALVE_POS_10000; + if (REF_VALVE_POS >= 0) { + valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_PLUS - VALVE_ELECTRIC_CENTER; } else { -// valve_pos_ref_dz = -REF_VALVE_POS_10000/10000.0f * (VALVE_MIN_POS-VALVE_DEADZONE_MINUS) + VALVE_DEADZONE_MINUS; - valve_pos_ref_dz = REF_VALVE_POS_10000; + valve_pos_ref_dz = REF_VALVE_POS + VALVE_DEADZONE_MINUS - VALVE_ELECTRIC_CENTER; } valve_pos_err = (float) (valve_pos_ref_dz - valve_pos.sen); valve_pos_err_diff = valve_pos_err - valve_pos_err_old; @@ -547,7 +544,7 @@ for (int ij = 0; ij<150; ij++) { ADC3->CR2 |= 0x40000000; - LVDT_new = ((float)ADC3->DR) - 2047.5f; + LVDT_new = ((float)ADC3->DR); LVDT_sum = LVDT_sum + LVDT_new; } @@ -836,8 +833,6 @@ ID_index = 0; CONTROL_UTILITY_MODE = MODE_NO_ACT; } - - break; } case MODE_DDV_DEADZONE_AND_CENTER: { @@ -1076,7 +1071,7 @@ valve_pos_raw.ref = (float)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f; } - VALVE_POS_CONTROL(valve_pos_raw.ref); + VALVE_POS_CONTROL_DZ(valve_pos_raw.ref); ref_array[cnt_step_test] = valve_pos_ref; if(valve_pos.sen >= (float) VALVE_ELECTRIC_CENTER) { @@ -1111,7 +1106,7 @@ valve_pos_raw.ref = (double)VALVE_ELECTRIC_CENTER - (float)valve_pos_ref * ((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER)/10000.0f; } - VALVE_POS_CONTROL(valve_pos_raw.ref); + VALVE_POS_CONTROL_DZ(valve_pos_raw.ref); ref_array[cnt_freq_test] = valve_pos_ref; // if(value>=(float) VALVE_ELECTRIC_CENTER) { @@ -1436,7 +1431,13 @@ if (((OPERATING_MODE&0b110)>>1) == 0 || ((OPERATING_MODE&0b110)>>1) == 1) { //Moog Valve or KNR Valve CAN_TX_PWM((int16_t)(cur.sen/mA_PER_pulse)); } else { - CAN_TX_PWM((int16_t)(valve_pos.sen)); + float valve_pos_can = 0.0f; + if(valve_pos.sen >= VALVE_ELECTRIC_CENTER) { + valve_pos_can = 10000.0f*((float)valve_pos.sen-(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MAX_POS-(float)VALVE_ELECTRIC_CENTER); + } else { + valve_pos_can = -10000.0f*((float)valve_pos.sen -(float)VALVE_ELECTRIC_CENTER)/((float)VALVE_MIN_POS-(float)VALVE_ELECTRIC_CENTER); + } + CAN_TX_PWM((int16_t)(valve_pos_can)); } }