Department of Electrical Eng University of Moratuwa
/
Vibration_Analysis_Force_Control
Force controlled vibration analysis
main.cpp
- Committer:
- eembed
- Date:
- 2019-08-01
- Revision:
- 0:5459cdde6298
- Child:
- 1:db36f62f783b
File content as of revision 0:5459cdde6298:
#include "mbed.h" #include "rtos.h" #include "SDFileSystem.h" #include "qeihw.h" #define Kp 80000.0 //8000.0 //max 100,000 //7000 //100000 #define Kv 50.0 //50.0 //500.0 //500 #define Gd 300.0 //200 #define PI 3.141592653 QEIHW qei_s(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE ); //LocalFileSystem local("local"); int position_m = 0; int position = 0; int position_s = 0; float encoder_m_prv=0.0,encoder_s_prv=0.0; float const G_filcon_I1_m = 350.0; float const G_filcon_I1_s = 350.0; float I1_act_m=0.0; float I1_act_s=0.0; // Mutex stdio_mutex; // Configuring two encoder modules void ethernet_init(); Ethernet eth; //Variable for get angle from ethernet char buf[0x600]; float recv_m_angle = 0.0; float recv_s_angle = 0.0; float inc_now = 0.0, inc_pre = 0.0; // Safety for mbed unused pins DigitalIn safety_19(p19); DigitalIn safety_20(p20); DigitalIn safety_25(p25); DigitalIn safety_26(p26); PwmOut pwm_m_clk(p21); // clockwise rotation pwm pin for MASTER PwmOut pwm_m_anticlk(p22); // anti - clockwise rotation pwm pin for MASTER PwmOut pwm_s_clk(p23); // clockwise rotation pwm pin for SLAVE PwmOut pwm_s_anticlk(p24); // anti-clockwise rotation pwm pin for SLAVE DigitalOut Reset_AB_M(p27); DigitalOut Reset_CD_M(p28); DigitalOut Reset_AB_S(p29); DigitalOut Reset_CD_S(p30); DigitalIn M_Dir(p9); DigitalIn S_Dir(p10); AnalogIn current_sensor_m_p(p15); // current sensor input for MASTER positive AnalogIn current_sensor_m_n(p16); // current sensor input for MASTER negative AnalogIn current_sensor_s_p(p17); // current sensor input for SLAVE positive AnalogIn current_sensor_s_n(p18); // current sensor input for SLAVE negative DigitalOut led1(LED1); DigitalOut led3(LED3); Timer timer; // For the controller FILE *fp; Ticker ticker; //Mutex stdio_mutex; int dt_us= 150, ramp_time=0.0; // define main loop time in us float dt; //loop time in seconds for calculations int counter_time; int counter=0; int counter_old=0; //Current Sensor Directions int Master_Direction=0; int Slave_Direction = 0; // Encoder Constants float const encoder_pulses_s = 2400.0; // PID parameters for Current - Loop //float const Ikp_m = 1.0, Iki_m =2.0, Ikd_m = 0.01; //float const Ikp_m = 0.01/*0.09*/, Iki_m =0.00, Ikd_m = 0.001; //float const Ikp_s = 0.04/*4.5*/, Iki_s = 0.00, Ikd_s = 0.01; float const Ikp_m = 0.026/*0.2*/, Iki_m =0.00, Ikd_m = 0.0001; //j=0.0000720,kp=0.026 float const Ikp_s = 0.026/*0.2*/, Iki_s = 0.00, Ikd_s = 0.0001; float I_ref_m = 0.0, I_err_m = 0.0, I_res_m = 0.0, I_tmp_m = 0.0, tem_I_m = 0.0, d_I_m = 0.0,I_ref_m1 = 0.0; float I_ref_s = 0.0, I_err_s = 0.0, I_res_s = 0.0, I_tmp_s = 0.0, tem_I_s = 0.0, d_I_s = 0.0,I_ref_s1 = 0.0; //some may not use.. float v_ref_m = 00.0, v_err_m = 0.0, v_res_m = 0.0,v_res_m_rpm = 0.0, v_tmp_m = 0.0, tem_v_m = 0.0, d_v_m = 0.0; float v_ref_s = 00.0, v_err_s = 0.0, v_res_s = 0.0,v_res_s_rpm = 0.0, v_tmp_s = 0.0, tem_v_s = 0.0, d_v_s = 0.0; float De_m=0.0, dx_res_m = 0.0, prev_v_err_m=0.0,I_v_err_m=0.0,de_m =0.0; float De_s=0.0, dx_res_s = 0.0, prev_v_err_s=0.0,I_v_err_s=0.0,de_s =0.0; float duty_m = 0.0; // PWM duty for master float duty_s = 0.0; float const G_filcon_I_m = 100.0; // Low pass filter gain for Current - loop default 1 float const G_filcon_I_s = 100.0; float const G_filcon_v_m = 100.0; // Low pass filter gain Velocity estimation float const G_filcon_v_s = 100.0; float I_act_m = 0.0; // Storing actual current flow float I_act_s = 0.0; float x_res_m_prv = 0.0; // Parameters to calculate current rotational speed float x_res_s_prv = 0.0; float encoder_m_now = 0.0; float encoder_s_now = 0.0; // Motor Constant and Inertia float const J_const_m = 0.0000720;//0.0000010;// //0.0000800;//0.000072 float const J_const_s = 0.0000720;//0.0000010;// //0.0000800; float const Kt_const_m = 0.135; float const Kt_const_s = 0.135;//0.134 float const Kt_constinv_m = 7.407;//1.43; float const Kt_constinv_s = 7.463;//1.43; float ddx_ref_m=0.0, ddx_ref_s=0.0; float tmp_m = 0.0,ob_sum_m = 0.0; float tmp_s = 0.0,ob_sum_s = 0.0; float i_com_m = 0.0; float i_com_s = 0.0; float fric_m = 0.0,fric_s = 0.0,i_rto_m = 0.0,i_rto_s = 0.0; float x_res_m = 0.0; float x_res_s = 0.0; float ve_sum_m = 0.0; float ve_sum_s = 0.0; float pwm_I_M= 0.0; float pwm_I_S= 0.0; float pid_V_I_M= 0.0; float pid_V_I_S= 0.0; float temp_a=0.0; float temp_b=0.0; float temp_a2=0.0; float temp_b2=0.0; float g_a1=1.0; float g_b1=1.4; float g_b2=1.4; float x_res_s_filter=0.0; float x_res_m_filter=0.0; float dx_res_s_filter=0.0; float dx_res_m_filter=0.0; float ob_sum_m1=0.0; float ob_sum_s1=0.0,torque_dob_m=0.0,torque_dob_s=0.0; //float error=0.0; void pwm_init(void) { pwm_m_clk.period_us(10); pwm_m_anticlk.period_us(10); pwm_s_anticlk.period_us(10); pwm_s_clk.period_us(10); pwm_m_clk.write(0.0f); // Set the ouput duty-cycle, specified as a percentage (float) pwm_m_anticlk.write(0.0f); pwm_s_anticlk.write(0.0f); pwm_s_clk.write(0.0f); Reset_AB_M = 1; //ENABLE RUNNING MODE (H BRIDGE ENABLE) Reset_CD_M = 1; Reset_AB_S = 1; Reset_CD_S = 1; } void velocity_m() { int size2 = eth.receive(); if(size2 > 0) { eth.read(buf, size2); memcpy(&recv_m_angle, buf, sizeof(float)); x_res_m = recv_m_angle; } ve_sum_m += dx_res_m*dt; dx_res_m =G_filcon_v_m*( x_res_m-ve_sum_m); } void velocity_s() { int32_t temp; position = 0; float d_v_err_s=0.0; qei_s.SetDigiFilter(480UL); qei_s.SetMaxPosition(0xFFFFFFFF); position = qei_s.GetPosition(); x_res_s = -1.0*position * 2.0 * PI / encoder_pulses_s; ve_sum_s += dx_res_s*dt; dx_res_s = G_filcon_v_s*(x_res_s-ve_sum_s); } void current_pid_m(){ Master_Direction = M_Dir.read(); if(Master_Direction == 0){ //master clockwise I_res_m = current_sensor_m_p.read(); I1_act_m = 1.0*((I_res_m*3.3/0.717075441532258) ); //0.74787687701613 }else if(Master_Direction == 1) { //master anticlockwise I_res_m = current_sensor_m_n.read(); I1_act_m = -1.0*((I_res_m*3.3)/0.717075441532258); //0.713239227822580 } //I_act_m += G_filcon_I1_m*(I1_act_m-I_act_m)*dt; I_act_m = 1*I1_act_m; I_err_m = I_ref_m - I_act_m; I_tmp_m += (Iki_m * dt * I_err_m); tem_I_m += G_filcon_I_m*d_I_m*dt; d_I_m = I_err_m - tem_I_m; pwm_I_M=((I_err_m * Ikp_m) + I_tmp_m + (d_I_m * Ikd_m)); } void current_pid_s(){ Slave_Direction = S_Dir.read(); if(Slave_Direction == 0){ //slave clockwise I_res_s = current_sensor_s_p.read(); I1_act_s = 1.0*((I_res_s*3.3)/0.717075441532258 ); //0.717075441532258 }else{ I_res_s = current_sensor_s_n.read(); //slave anticlockwise I1_act_s = -1.0*((I_res_s*3.3)/0.717075441532258 ); //0.724138445564516 } //I_act_s += G_filcon_I1_s*(I1_act_s-I_act_s)*dt; I_act_s = I1_act_s; I_err_s = I_ref_s - I_act_s; I_tmp_s += Iki_s * dt * I_err_s; tem_I_s += G_filcon_I_s*d_I_s*dt; d_I_s = I_err_s - tem_I_s; pwm_I_S=((I_err_s * Ikp_s) + I_tmp_s + (d_I_s * Ikd_s)); } void Disob() { tmp_m = Gd*J_const_s*dx_res_m; //ob_sum_m += (Gd*(Kt_const_s*I_act_m+tmp_m-ob_sum_m)*dt); ob_sum_m1 = Kt_const_m*I_act_m+tmp_m; ob_sum_m += Gd*(ob_sum_m1-ob_sum_m)*dt; i_com_m = (ob_sum_m - tmp_m)*Kt_constinv_m; //read current //i_com_m=0.0; torque_dob_m= i_com_m*Kt_const_m; fric_m = 0.0; i_rto_m = (i_com_m-fric_m); //friction should be in current terms tmp_s = Gd*J_const_s*dx_res_s; //ob_sum_s += (Gd*(Kt_const_s*I_act_s+tmp_s-ob_sum_s)*dt); ob_sum_s1 = Kt_const_s*I_act_s+tmp_s; ob_sum_s += Gd*(ob_sum_s1-ob_sum_s)*dt; i_com_s = (ob_sum_s - tmp_s)*Kt_constinv_s; //read current //i_com_s = 0.0; torque_dob_s = i_com_s*Kt_const_s; fric_s = 0.0; i_rto_s = (i_com_s-fric_s); //friction should be in current terms } int Controller(void) { ddx_ref_m = Kp*(x_res_s-x_res_m) + Kv*(dx_res_s-dx_res_m); I_ref_m1 = Kt_constinv_m*J_const_m*ddx_ref_m; I_ref_m = I_ref_m1 + i_com_m - (i_rto_m+i_rto_s); //I_ref_m = 0.1; ddx_ref_s = Kp*(x_res_m-x_res_s) + Kv*(dx_res_m-dx_res_s); I_ref_s1 = Kt_constinv_s*J_const_s*ddx_ref_s; I_ref_s =I_ref_s1 + i_com_s - (i_rto_s+i_rto_m); //I_ref_s = 0.1; return 0; } void PWM_Generator_m() { duty_m = pwm_I_M; if (duty_m> 0.0) { if (duty_m > 0.5) { duty_m = 0.5; } pwm_m_clk = 0.0; pwm_m_anticlk = duty_m; } if (duty_m < 0.0) { if (duty_m < -0.5) { duty_m = -0.5; } pwm_m_anticlk = 0.0; pwm_m_clk = -1.0 * duty_m; } } void PWM_Generator_s() { duty_s = pwm_I_S; if (duty_s > 0.0) { if (duty_s > 0.5) { duty_s = 0.5; } pwm_s_clk = 0.0; pwm_s_anticlk = duty_s; } if (duty_s < 0.0) { if (duty_s < -0.5) { duty_s = -0.5; } pwm_s_anticlk = 0.0; pwm_s_clk = -1.0 * duty_s; } } void cleanup_module(void){ pwm_m_clk = 0.0; // pwm cleanup module pwm_m_anticlk = 0.0; pwm_s_anticlk = 0.0; pwm_s_clk = 0.0; Reset_AB_M = 0; //RESET H BRIDGE Reset_CD_M = 0; Reset_AB_S = 0; Reset_CD_S = 0; led1=0; led3=0; } //RTOS void Control_body() { // Control Part - main code Controller(); velocity_m(); velocity_s (); current_pid_m(); current_pid_s (); PWM_Generator_m(); PWM_Generator_s(); Disob(); //led1=!led1; counter++; } void thread_2(void const *argument){ led1=1; SDFileSystem sd(p5, p6, p7, p8, "sd"); FILE *fp = fopen("/sd/BC.txt", "w"); if(fp == NULL) { for(int i=0;i<5;i++){ led3=!led3; wait(1.0); } } while(counter<80000){ if(counter>=(counter_old+100)){ //fprintf(fp, "%d %f %f %f %f %f %f %f %f \n",timer.read_us(), x_res_m,x_res_s,I_act_m,I_act_s,torque_dob_s,torque_dob_m,i_com_s,i_com_m); fprintf(fp, "%d %f %f %f %f \n",timer.read_us(),torque_dob_m,torque_dob_s,x_res_m,x_res_s); counter_old=counter; led3=!led3; } } fclose(fp); timer.stop(); cleanup_module(); ticker.detach (); wait(1.0); } void ethernet_init(){ eth.set_link(Ethernet::HalfDuplex100); wait_ms(1000); // Needed after startup. if(eth.link()) { for(int i=0;i<3;i++){ led3=!led3; wait(1.0); } } } int main() { ethernet_init(); pwm_init(); timer.start(); dt=dt_us/1000000.0; ticker.attach_us(&Control_body, dt_us); Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL); }