Department of Electrical Eng University of Moratuwa
/
Vibration_Analysis_Force_Control
Force controlled vibration analysis
main.cpp
- Committer:
- eembed
- Date:
- 2019-08-30
- Revision:
- 3:a921792d9913
- Parent:
- 1:db36f62f783b
File content as of revision 3:a921792d9913:
/* * Force Controlled Vibration Analysis - * * 22.08.2019 * Theekshana */ #include "mbed.h" #include "rtos.h" #include "SDFileSystem.h" #include "qeihw.h" //---------------------------=Communication Pins=------------------------------- //Serial pc(USBTX, USBRX); DigitalOut led3(LED3); DigitalOut led1(LED1); SDFileSystem sd(p5, p6, p7, p8, "sd"); // pintout for sd card //--------------------------=Motor Driver Controller Pin =----------------------------- DigitalOut Reset_AB(p29); // H bridge enable DigitalOut Reset_CD(p30); DigitalOut Reset_AB_s(p27); DigitalOut Reset_CD_s(p28); PwmOut pwm_clk_s(p21); // clockwise rotation pwm pin for Slave PwmOut pwm_anticlk_s(p22); PwmOut pwm_clk(p23); // clockwise rotation pwm pin for Master PwmOut pwm_anticlk(p24); //--------------------------=Current Sensor MBED Pins=-------------------------- int Master_Direction=0; int slave_Direction=0; DigitalIn M_Dir(p10); DigitalIn S_Dir(p9); AnalogIn current_sensor_s_p(p15); // current sensor input for MASTER positive AnalogIn current_sensor_s_n(p16); AnalogIn current_sensor_m_p(p17); // current sensor input for MASTER positive AnalogIn current_sensor_m_n(p18); // current sensor input for MASTER negative float I_res_m = 0.0; float I1_act_m=0.0; float I_msrd=0.0; float G_Cfilter=30.0; //--------------------------=Ethernet Variable=--------------------------------- Ethernet eth; //--------------------------=Thread Variables=----------------------------------- Ticker time_up; float startTime=0.0; float tempTime = 0.0; float endTime=0.0; Timer timer; float preTime = 0.0; //--------------------------=Velocity reading variables=------------------------- char buf[0x600]; float recv_angle = 0.0; float x_res = 0.0; float dt = 0.000001*200.0; float ve_sum = 0.0; float dx_res = 0.0; float G_filter_v = 150.0; float duty = 0.0; //--------------------------=DoB variables=-------------------------------------- float K_tn=0.135; float J_n = 0.0000720; float B = 0.0; float g_dis = 1.0; float D_DoB = 0.0; float D_BFilter = 0.0; float D_AFilter = 0.0; float I_DoB = 0.0; float I_sin = 0.0; float I_sin_prev = 0.0; float I_DoB_prev = 0.0; //--------------------------=RTOB Variables=------------------------------------- float RT_BFilter=0.0; float RT_AFilter = 0.0; float RTOB = 0.0; //--------------------------=Force Controller Variables=------------------------- float T_f = 0.025; float K_pf = 500.0; // tested value for smooth oparation float K_df = 0.0; float K_if = 0.0; float I_error = 0.0; float I_err_sum = 0.0; float I_err_prev = 0.0; float I_cmd = 1.3; float I_PID = 0.0; float I_ref = 0.0; //-------------------------=slave current controller variables=----------------- int count = 0; float f = 10.0; float K_pis =10.0; float K_dis = 0.0; float K_iis = 0.0; float I_error_s = 0.0; float I_ref_s = 0.0; float I_err_sum_s = 0.0; float I_PID_s = 0.0; float I_err_prev_s = 0.0; float duty_s = 0.0; float I_sin_gen = 0.0; float I_res_s = 0.0; float I1_act_s = 0.0; float I_msrd_s = 0.0; //--------------------------=Fucion declaration=-------------------------------- void controlLoop(); void readVelocity(); void readCurrent(); void CController(); void DoB(); void motorPWM_init(); void motorPWM(); void slaveCurrentRead(); void slaveCurrentController(); void motorpwm_s(); void thread_2(void const *argument); void sd_card_write_test(); void sd_card_write(); void ethernet_init(); //----------------------------------=Main Loop=--------------------------------- int main() { sd_card_write_test(); ethernet_init(); motorPWM_init(); timer.start(); time_up.attach(&controlLoop, dt); Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL); } //--------------------------=Main Control Loop=--------------------------------- void controlLoop(){ /*if (x_res>0.0) { T_f = -0.025; } else { T_f = 0.025; }*/ count = count+1; I_sin_gen = 0.25*sin(2*3.14159*dt*f*count); readVelocity(); readCurrent(); CController(); DoB(); motorPWM(); slaveCurrentRead(); slaveCurrentController(); motorpwm_s(); } //--------------------------=Read Velocity=------------------------------------- void readVelocity(){ int size2 = eth.receive(); if(size2 > 0) { eth.read(buf, size2); memcpy(&recv_angle, buf, sizeof(float)); x_res = -1*recv_angle; } ve_sum += dx_res*dt; dx_res =G_filter_v*( x_res-ve_sum); } //--------------------------=Current Meassurement=------------------------------ void readCurrent() { Master_Direction = M_Dir.read(); //master clockwise 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 }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.7); //0.713239227822580 } I_msrd += G_Cfilter*(I1_act_m-I_msrd)*dt; } //--------------------------=Velocity Controller=------------------------------- void CController() { I_ref = I_cmd;// -I_sin_gen; I_error = I_ref - I_msrd; I_err_sum += I_error*dt; I_PID = (K_pf*I_error)+(K_df*(I_error-I_err_prev)/dt) + (K_if*I_err_sum); I_err_prev = I_error; } //--------------------------=Distarbance Observer=------------------------------ void DoB() { D_BFilter=(I_msrd*K_tn) + (dx_res*J_n*g_dis); 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=------------------------------ RT_BFilter = D_BFilter - (T_f+B*dx_res); RT_AFilter += g_dis*(RT_BFilter-RT_AFilter)*dt; RTOB = RT_AFilter - (dx_res*J_n*g_dis); } //--------------------------=Motor PWM Initialization=-------------------------- void motorPWM_init() { pwm_clk.period_us(10); pwm_clk_s.period_us(10); pwm_anticlk_s.period_us(10); pwm_anticlk.period_us(10); pwm_clk.write(0.0f); pwm_anticlk.write(0.0f); pwm_clk_s.write(0.0f); pwm_anticlk_s.write(0.0f); Reset_AB = 1; Reset_CD = 1; Reset_AB_s =1; Reset_CD_s =1; } //--------------------------=Motor PWM Generation=------------------------------ void motorPWM() { duty = I_PID; if (duty> 0.0) { if (duty > 0.9) { duty = 0.9; } pwm_clk = 0.0; pwm_anticlk = duty; } if (duty < 0.0) { if (duty< -0.9) { duty = -0.9; } pwm_anticlk = 0.0; pwm_clk = -1.0 * duty; } } //---------------------------=Slave motor=-------------------------------------- //---------------------------=Slave motor current reading=---------------------- void slaveCurrentRead() { slave_Direction = S_Dir.read(); 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 }else if(slave_Direction == 1) { //master anticlockwise I_res_s = current_sensor_s_n.read(); I1_act_s = 1.0*((I_res_s*3.3)/0.7); //0.713239227822580 } I_msrd_s += G_Cfilter*(I1_act_s-I_msrd_s)*dt; } //-----------------------=slaveCurrentController=------------------------------- void slaveCurrentController() { I_ref_s = I_sin_gen; // 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_err_prev_s = I_error_s; } //----------------------=Slave Motor run=--------------------------------------- void motorpwm_s(){ duty_s = I_PID_s; if (duty_s> 0.0) { if (duty_s > 0.9) { duty_s = 0.9; } pwm_clk_s = 0.0; pwm_anticlk_s = duty_s; } if (duty_s < 0.0) { if (duty_s< -0.9) { duty_s = -0.9; } pwm_anticlk_s = 0.0; pwm_clk_s = -1.0 * duty_s; } } //--------------------------=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,I_cmd,I_ref,I_msrd,x_res); sd_card_write(); } } //--------------------------=File print test=----------------------------------- void sd_card_write_test() { mkdir("/sd/mydir", 0777); FILE *fp = fopen("/sd/mydir/sdtest.txt", "w"); if(fp == NULL) { error("Could not open file for write\n"); } 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=---------------------------------------- void sd_card_write() { //led1=!led1; FILE *fp = fopen("/sd/mydir/sdtest.txt","a"); fprintf(fp,"%d\t %f\t %f\t \r\n",count,I_sin,I_sin_gen); fclose(fp); } //--------------------------=Ethernet Initialization=--------------------------- 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); } } }