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;