![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
For LIGHT
Diff: main.cpp
- Revision:
- 51:b46bed7fec80
- Parent:
- 50:3c630b5eba9f
- Child:
- 52:8ea76864368a
diff -r 3c630b5eba9f -r b46bed7fec80 main.cpp --- a/main.cpp Thu Feb 13 05:16:15 2020 +0000 +++ b/main.cpp Wed Feb 19 00:44:07 2020 +0000 @@ -344,9 +344,8 @@ TIMER INTERRUPT *******************************************************************************/ -//unsigned long CNT_TMR4 = 0; -float FREQ_TMR4 = (float)FREQ_10k; -float DT_TMR4 = (float)DT_10k; +float FREQ_TMR4 = (float)FREQ_20k; +float DT_TMR4 = (float)DT_20k; extern "C" void TIM4_IRQHandler(void) { @@ -369,7 +368,7 @@ //Pressure sensor A ADC1->CR2 |= 0x40000000; // adc _ 12bit //while((ADC1->SR & 0b10)); - float alpha_update_pres_A = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*1000.0f)); + float alpha_update_pres_A = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*100.0f)); // float pres_A_new = ((float)ADC1->DR - PRES_A_NULL) / PRES_SENSOR_A_PULSE_PER_BAR; float pres_A_new = ((float)ADC1->DR); pres_A.sen = pres_A.sen*(1.0f-alpha_update_pres_A)+pres_A_new*(alpha_update_pres_A); @@ -389,7 +388,7 @@ // a1=ADC2->DR; //int raw_cur = ADC3->DR; //while((ADC3->SR & 0b10)); - float alpha_update_cur = 1.0f/(1.0f+(FREQ_TMR4/2.0f)/(2.0f*3.14f*200.0f)); // f_cutoff : 500Hz + float alpha_update_cur = 1.0f/(1.0f + FREQ_TMR4/(2.0f*3.14f*500.0f)); // f_cutoff : 500Hz float cur_new = ((float)ADC3->DR-2048.0f)*20.0f/4096.0f; // unit : mA cur.sen=cur.sen*(1.0f-alpha_update_cur)+cur_new*(alpha_update_cur); //cur.sen = raw_cur; @@ -464,8 +463,8 @@ // torque feedback torq.err = torq_ref - torq.sen; //[pulse] torq.err_sum += torq.err/(float) TMR_FREQ_5k; //[pulse] - if (torq.err_sum > 1000) torq.err_sum = 1000; - if (torq.err_sum<-1000) torq.err_sum = -1000; +// if (torq.err_sum > 1000) torq.err_sum = 1000; +// if (torq.err_sum<-1000) torq.err_sum = -1000; VALVE_POS_RAW_FORCE_FB = alpha_trans*(((float) P_GAIN_JOINT_TORQUE * torq.err + (float) I_GAIN_JOINT_TORQUE * torq.err_sum) /(float) TORQUE_SENSOR_PULSE_PER_TORQUE * 0.01f @@ -476,6 +475,23 @@ } else { valve_pos.ref = VALVE_POS_RAW_FORCE_FB + VALVE_DEADZONE_MINUS; } + + if(I_GAIN_JOINT_TORQUE != 0){ + double Ka = 1.0f / (double) I_GAIN_JOINT_TORQUE * (float) TORQUE_SENSOR_PULSE_PER_TORQUE * 100.0f; + if(valve_pos.ref>VALVE_MAX_POS){ + double valve_pos_rem = valve_pos.ref - VALVE_MAX_POS; + valve_pos_rem = valve_pos_rem * Ka; + valve_pos.ref = VALVE_MAX_POS; + torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k; + } + else if(valve_pos.ref < VALVE_MIN_POS){ + double valve_pos_rem = valve_pos.ref - VALVE_MIN_POS; + valve_pos_rem = valve_pos_rem * Ka; + valve_pos.ref = VALVE_MIN_POS; + torq.err_sum = torq.err_sum - valve_pos_rem/(float) TMR_FREQ_5k; + } + } + VALVE_POS_CONTROL(valve_pos.ref); break;