Department of Electrical Eng University of Moratuwa
/
Vibration_Analysis_Force_Control
Force controlled vibration analysis
Diff: main.cpp
- Revision:
- 4:8432d3d42835
- Parent:
- 2:409ee7fcbd8a
- Child:
- 5:9ab19c63203e
--- a/main.cpp Fri Aug 30 12:11:12 2019 +0000 +++ b/main.cpp Fri Sep 06 08:59:53 2019 +0000 @@ -12,7 +12,7 @@ //---------------------------=Communication Pins=------------------------------- -Serial pc(USBTX, USBRX); +//Serial pc(USBTX, USBRX); DigitalOut led3(LED3); DigitalOut led1(LED1); SDFileSystem sd(p5, p6, p7, p8, "sd"); // pintout for sd card @@ -47,6 +47,10 @@ //--------------------------=Ethernet Variable=--------------------------------- Ethernet eth; +float I_sin = 0.0; +float I_DoB_prev = 0.0; +float I_sin_prev =0.0; + //--------------------------=Thread Variables=----------------------------------- @@ -73,7 +77,7 @@ float B = 0.0; float g_dis = 100.0; -float g_Tdis = 70.0; +float g_Tdis = 75.0; float D_DoB = 0.0; float D_BFilter = 0.0; float D_AFilter = 0.0; @@ -85,11 +89,20 @@ float RT_AFilter = 0.0; float RTOB = 0.0; +float K_pi = 40.0; // tested value for smooth oparation +float K_di = 0.0; +float K_ii = 0.0; +float I_error = 0.0; +float I_err_sum = 0.0; +float I_err_prev = 0.0; + +float I_PID = 0.0; + //--------------------------=Force Controller Variables=------------------------- float F_ref = 500.0; -float T_ref = 0.0; +float T_ref = 0.15; float T_f = 0.025; -float K_pf = 25000.0; // tested value for smooth oparation +float K_pf = 12000.0; // tested value for smooth oparation float K_df = 0.0; float K_if = 0.0; float F_error = 0.0; @@ -124,6 +137,7 @@ void forceController(); void DoB(); void motorPWM_init(); + void motorPWM(); void slaveCurrentRead(); @@ -140,12 +154,12 @@ //----------------------------------=Main Loop=--------------------------------- int main() { + ethernet_init(); sd_card_write_test(); FILE *fp = fopen("/sd/mydir/sdtest.txt","w"); - ethernet_init(); motorPWM_init(); time_up.attach(&controlLoop, dt); - //Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL); + while(1) { //pc.printf("%d\t %f\t %f\t %f\t %f\t\r\n",count,T_ref,RTOB,I_msrd,x_res); @@ -165,16 +179,18 @@ } count = count+1; + I_ref_s = 0.2*sin(2*3.14159*dt*f*count); // 2*3.14159*dt = readVelocity(); readCurrent(); forceController(); - DoB(); + DoB(); motorPWM(); slaveCurrentRead(); slaveCurrentController(); motorpwm_s(); } + //--------------------------=Read Velocity=------------------------------------- void readVelocity(){ int size2 = eth.receive(); @@ -196,13 +212,13 @@ if(Master_Direction == 0) { I_res_m = current_sensor_m_p.read(); - I1_act_m = -1.0*((I_res_m*3.3/0.7) ); //0.74787687701613 //0.717075441532258 + I1_act_m = -1.0*(I_res_m*4.71428); //0.74787687701613 //0.717075441532258 - }else if(Master_Direction == 1) { //master anticlockwise + }else { //master anticlockwise I_res_m = current_sensor_m_n.read(); - I1_act_m = 1.0*((I_res_m*3.3)/0.7); //0.713239227822580 + I1_act_m = (I_res_m*4.71428); //0.713239227822580 } - I_msrd += G_Cfilter*(I1_act_m-I_msrd)*dt; + I_msrd += 30.0*(I1_act_m-I_msrd)*dt; } @@ -210,13 +226,14 @@ //--------------------------=Velocity Controller=------------------------------- void forceController() { - T_ref = F_ref*6.1/10000.0; + //T_ref = F_ref*6.1/10000.0; F_error = T_ref - RTOB; F_err_sum += F_error*dt; - F_PID = (K_pf*F_error)+(K_df*(F_error-F_err_prev)/dt) + (K_if*F_err_sum); + F_PID = (K_pf*F_error);//+(K_df*(F_error-F_err_prev)/dt) + (K_if*F_err_sum); F_err_prev = F_error; I_cmd = F_PID*J_n/K_tn; - I_ref = I_cmd +I_DoB; + I_ref = I_cmd +(I_DoB); + } //--------------------------=Distarbance Observer=------------------------------ @@ -226,6 +243,9 @@ D_AFilter += g_dis*(D_BFilter-D_AFilter)*dt; D_DoB = D_AFilter - (dx_res*J_n*g_dis); I_DoB = D_DoB/K_tn; + /*I_sin = I_DoB-I_DoB_prev+ 0.999998*I_sin_prev; + I_DoB_prev = I_DoB; + I_sin_prev = I_sin;*/ //--------------------------=Reaction Force Observer=------------------------------ R_BFilter=(I_msrd*K_tn) + (dx_res*J_n*g_Tdis); @@ -258,19 +278,17 @@ duty = I_ref/5.1; if (duty> 0.0) { - if (duty > 1.0) + if (duty > 0.95) { - duty = 1.0; + duty = 0.95; } pwm_clk = 0.0; pwm_anticlk = duty; - } - - if (duty < 0.0) + }else { - if (duty< -1.0) + if (duty< -0.95) { - duty = -1.0; + duty = -0.95; } pwm_anticlk = 0.0; pwm_clk = -1.0 * duty; @@ -286,23 +304,22 @@ if(slave_Direction == 0) { I_res_s = current_sensor_s_p.read(); - I1_act_s = -1.0*((I_res_s*3.3/0.7) );//0.74787687701613 //0.717075441532258 + I1_act_s = -1.0*((I_res_s*4.71428) );//0.74787687701613 //0.717075441532258 - }else if(slave_Direction == 1) { //master anticlockwise + }else { //master anticlockwise I_res_s = current_sensor_s_n.read(); - I1_act_s = 1.0*((I_res_s*3.3)/0.7); //0.713239227822580 + I1_act_s = 1.0*((I_res_s*4.71428)); //0.713239227822580 } - I_msrd_s += G_Cfilter*(I1_act_s-I_msrd_s)*dt; + I_msrd_s += 30.0*(I1_act_s-I_msrd_s)*dt; //G_Cfilter = 30.00 } //-----------------------=slaveCurrentController=------------------------------- void slaveCurrentController() -{ - I_ref_s = 0.1*sin(2*3.14159*dt*f*count); // 2 x PI x t x +{ I_error_s = I_ref_s - I_msrd_s; I_err_sum_s += I_error_s*dt; - I_PID_s = (K_pis*I_error_s)+(K_dis*(I_error_s-I_err_prev_s)/dt) + (K_iis*I_err_sum_s); + I_PID_s = (K_pis*I_error_s);//+(K_dis*(I_error_s-I_err_prev_s)/dt) + (K_iis*I_err_sum_s); I_err_prev_s = I_error_s; } @@ -311,19 +328,19 @@ duty_s = I_PID_s; if (duty_s> 0.0) { - if (duty_s > 1.0) + if (duty_s > 0.95) { - duty_s = 1.0; + duty_s = 0.95; } pwm_clk_s = 0.0; pwm_anticlk_s = duty_s; } - if (duty_s < 0.0) + else { - if (duty_s< -1.0) + if (duty_s< -0.95) { - duty_s = -1.0; + duty_s = -0.95; } pwm_anticlk_s = 0.0; pwm_clk_s = -1.0 * duty_s; @@ -332,17 +349,6 @@ //--------------------------=Data writting to file=----------------------------- -//--------------------------=Printing to a file=-------------------------------- -void thread_2(void const *argument) -{ - while(1) - { - - //pc.printf("%f %f\n",I1_act_m,I_msrd); - //pc.printf("%d\t %f\t %f\t %f\t %f\t\r\n",count,T_ref,RTOB,I_msrd,x_res); - //sd_card_write(); - } -} //--------------------------=File print test=----------------------------------- @@ -357,7 +363,7 @@ fprintf(fp, "count \t x_res\t RTOB \n"); fclose(fp); - //fprintf(fp,"startTime\t I_ref\t I_msrd\t dx_res\t\n"); + } //--------------------------=File print=---------------------------------------- @@ -367,7 +373,7 @@ FILE *fp = fopen("/sd/mydir/sdtest.txt","a"); //setvbuf(fp, NULL, _IONBF, 0); //fwrite(&K_tn,1,sizeof(K_tn), fp); - fprintf(fp,"%d\t %f\t %f\t \r\n",count,x_res,RTOB); + fprintf(fp,"%d\t %f\t %f\t \r\n",count,x_res,RTOB); fclose(fp); }