main.cpp

00001 /*
00002 * Force Controlled Vibration Analysis - 
00003 * 
00004 * 22.08.2019
00005 * Theekshana
00006 */
00007  
00008 #include "mbed.h"
00009 #include "rtos.h"
00010 #include "SDFileSystem.h"
00011 #include "qeihw.h"
00012 
00013 //---------------------------=Communication Pins=-------------------------------
00014 //Serial pc(USBTX, USBRX);
00015 DigitalOut led3(LED3);
00016 DigitalOut led1(LED1);
00017 SDFileSystem sd(p5, p6, p7, p8, "sd");  // pintout for sd card
00018 
00019 //--------------------------=Motor Driver Controller Pin =-----------------------------
00020 DigitalOut  Reset_AB(p29);          // H bridge enable
00021 DigitalOut  Reset_CD(p30);
00022 DigitalOut  Reset_AB_s(p27);
00023 DigitalOut  Reset_CD_s(p28);
00024 
00025 PwmOut pwm_clk_s(p21);              // clockwise rotation pwm pin for Slave
00026 PwmOut pwm_anticlk_s(p22);          
00027 PwmOut pwm_clk(p23);                // clockwise rotation pwm pin for Master
00028 PwmOut pwm_anticlk(p24); 
00029 
00030 //--------------------------=Current Sensor MBED Pins=--------------------------
00031 int Master_Direction=0;
00032 int slave_Direction=0;
00033 
00034 DigitalIn M_Dir(p10);
00035 DigitalIn S_Dir(p9);
00036 AnalogIn current_sensor_s_p(p15); // current sensor input for MASTER positive
00037 AnalogIn current_sensor_s_n(p16);
00038 AnalogIn current_sensor_m_p(p17); // current sensor input for MASTER positive
00039 AnalogIn current_sensor_m_n(p18); // current sensor input for MASTER negative
00040 
00041 float I_res_m = 0.0;
00042 float  I1_act_m=0.0;
00043 float I_msrd=0.0;
00044 float G_Cfilter=30.0;
00045 
00046 //--------------------------=Ethernet Variable=---------------------------------
00047 Ethernet eth;
00048 
00049 
00050 //--------------------------=Thread Variables=-----------------------------------
00051 Ticker time_up;
00052 float startTime=0.0;
00053 float tempTime = 0.0;
00054 float endTime=0.0;
00055 Timer timer;
00056 float preTime = 0.0;
00057 
00058 //--------------------------=Velocity reading variables=-------------------------
00059 char buf[0x600];
00060 float recv_angle = 0.0;
00061 float x_res = 0.0;
00062 float dt = 0.000001*200.0;
00063 float ve_sum = 0.0;
00064 float dx_res = 0.0;
00065 float G_filter_v = 150.0;
00066 float duty = 0.0;
00067 
00068 //--------------------------=DoB variables=--------------------------------------
00069 float K_tn=0.135;
00070 float J_n = 0.0000720;
00071 float B = 0.0;
00072 
00073 float g_dis = 1.0;
00074 float D_DoB = 0.0;
00075 float D_BFilter = 0.0;
00076 float D_AFilter = 0.0;
00077 float I_DoB = 0.0;
00078 float I_sin = 0.0;
00079 float I_sin_prev = 0.0;
00080 float I_DoB_prev = 0.0;
00081 
00082 //--------------------------=RTOB Variables=-------------------------------------
00083 float RT_BFilter=0.0;
00084 float RT_AFilter = 0.0;
00085 float RTOB = 0.0;
00086 
00087 //--------------------------=Force Controller Variables=-------------------------
00088 
00089 
00090 float T_f = 0.025;
00091 float K_pf = 500.0;                       // tested value for smooth oparation
00092 float K_df = 0.0;
00093 float K_if = 0.0;
00094 float I_error = 0.0;
00095 float I_err_sum = 0.0;
00096 float I_err_prev = 0.0;
00097 float I_cmd = 1.3;
00098 float I_PID = 0.0;
00099 float I_ref = 0.0;
00100 
00101 //-------------------------=slave current controller variables=-----------------
00102 int count = 0;
00103 float f = 10.0;
00104 float K_pis =10.0;
00105 float K_dis = 0.0;
00106 float K_iis = 0.0;
00107 
00108 float I_error_s = 0.0;
00109 float I_ref_s = 0.0;
00110 float I_err_sum_s = 0.0;
00111 float I_PID_s = 0.0;
00112 float I_err_prev_s = 0.0;
00113 float duty_s = 0.0;
00114 float I_sin_gen = 0.0;
00115 float I_res_s  = 0.0;
00116 float I1_act_s = 0.0;
00117 float I_msrd_s = 0.0;
00118 
00119 
00120 //--------------------------=Fucion declaration=--------------------------------
00121 void controlLoop();
00122 void readVelocity();
00123 void readCurrent();
00124 void CController();
00125 void DoB();
00126 void motorPWM_init();
00127 void motorPWM();
00128 
00129 void slaveCurrentRead();
00130 void slaveCurrentController();
00131 void motorpwm_s();
00132 
00133 void thread_2(void const *argument);
00134 void sd_card_write_test();
00135 void sd_card_write();
00136 void ethernet_init();
00137 
00138 
00139 
00140 
00141 //----------------------------------=Main Loop=---------------------------------
00142 int main()
00143 {
00144     sd_card_write_test();
00145     ethernet_init();
00146     motorPWM_init();
00147     timer.start();
00148     time_up.attach(&controlLoop, dt); 
00149     Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
00150 }
00151 
00152 //--------------------------=Main Control Loop=---------------------------------
00153 void controlLoop(){
00154     /*if (x_res>0.0)
00155     {
00156         T_f = -0.025;
00157     }
00158     else
00159     {
00160         T_f = 0.025;
00161     }*/
00162     count = count+1;
00163     I_sin_gen = 0.25*sin(2*3.14159*dt*f*count); 
00164     readVelocity(); 
00165     readCurrent();
00166     CController();
00167     DoB();
00168     motorPWM();
00169     slaveCurrentRead();
00170     slaveCurrentController();
00171     motorpwm_s();
00172 }
00173 
00174 //--------------------------=Read Velocity=-------------------------------------
00175 void readVelocity(){
00176     int size2 = eth.receive();
00177     if(size2 > 0)
00178     {
00179         eth.read(buf, size2);
00180         memcpy(&recv_angle, buf, sizeof(float));
00181         x_res = -1*recv_angle; 
00182     } 
00183     ve_sum += dx_res*dt;
00184     dx_res =G_filter_v*( x_res-ve_sum);
00185 }
00186 
00187 //--------------------------=Current Meassurement=------------------------------
00188 void readCurrent()
00189 {
00190     Master_Direction = M_Dir.read();
00191     //master clockwise
00192     if(Master_Direction == 0)
00193     {                                
00194         I_res_m = current_sensor_m_p.read(); 
00195         I1_act_m = -1.0*((I_res_m*3.3/0.7) );    //0.74787687701613 //0.717075441532258
00196          
00197     }else if(Master_Direction == 1) { //master anticlockwise 
00198         I_res_m =  current_sensor_m_n.read();
00199         I1_act_m = 1.0*((I_res_m*3.3)/0.7);   //0.713239227822580 
00200     }
00201     I_msrd += G_Cfilter*(I1_act_m-I_msrd)*dt;
00202 }
00203 
00204 
00205 
00206 //--------------------------=Velocity Controller=-------------------------------
00207 void CController()
00208 {
00209     I_ref = I_cmd;// -I_sin_gen;   
00210     I_error = I_ref - I_msrd;
00211     I_err_sum += I_error*dt;
00212     I_PID = (K_pf*I_error)+(K_df*(I_error-I_err_prev)/dt) + (K_if*I_err_sum);  
00213     I_err_prev = I_error;
00214 }
00215 
00216 //--------------------------=Distarbance Observer=------------------------------
00217 void DoB()
00218 {
00219     D_BFilter=(I_msrd*K_tn) +  (dx_res*J_n*g_dis);
00220     D_AFilter += g_dis*(D_BFilter-D_AFilter)*dt;
00221     D_DoB = D_AFilter - (dx_res*J_n*g_dis);
00222     I_DoB = D_DoB/K_tn; 
00223     I_sin = I_DoB-I_DoB_prev+ 0.999998*I_sin_prev;
00224     I_DoB_prev = I_DoB;
00225     I_sin_prev = I_sin;
00226 //--------------------------=Reaction Force Observer=------------------------------  
00227     RT_BFilter = D_BFilter - (T_f+B*dx_res);
00228     RT_AFilter += g_dis*(RT_BFilter-RT_AFilter)*dt;
00229     RTOB =  RT_AFilter - (dx_res*J_n*g_dis);
00230 }
00231 
00232 
00233 //--------------------------=Motor PWM Initialization=--------------------------
00234 void motorPWM_init()
00235 {
00236     pwm_clk.period_us(10);
00237     pwm_clk_s.period_us(10);
00238     pwm_anticlk_s.period_us(10);
00239     pwm_anticlk.period_us(10);
00240     pwm_clk.write(0.0f);       
00241     pwm_anticlk.write(0.0f);
00242     pwm_clk_s.write(0.0f);       
00243     pwm_anticlk_s.write(0.0f);
00244     Reset_AB = 1;               
00245     Reset_CD = 1;
00246     Reset_AB_s =1;
00247     Reset_CD_s =1;
00248 }
00249 
00250 //--------------------------=Motor PWM Generation=------------------------------
00251 void motorPWM()
00252 {
00253      duty = I_PID;
00254      if (duty> 0.0)
00255      {
00256         if (duty > 0.9)
00257         {
00258             duty = 0.9;
00259         }
00260         pwm_clk = 0.0;
00261         pwm_anticlk = duty;
00262     }
00263 
00264     if (duty < 0.0)
00265     {
00266         if (duty< -0.9)
00267         {
00268             duty = -0.9;
00269         }
00270         pwm_anticlk = 0.0;
00271         pwm_clk = -1.0 * duty;
00272     }
00273 }
00274 
00275 //---------------------------=Slave motor=--------------------------------------
00276 //---------------------------=Slave motor current reading=----------------------
00277 void slaveCurrentRead()
00278 {
00279     slave_Direction = S_Dir.read();
00280                                                     
00281     if(slave_Direction == 0)                
00282     {                                
00283         I_res_s = current_sensor_s_p.read();        
00284         I1_act_s = -1.0*((I_res_s*3.3/0.7) );//0.74787687701613 //0.717075441532258
00285          
00286     }else if(slave_Direction == 1) { //master anticlockwise 
00287         I_res_s =  current_sensor_s_n.read();
00288         I1_act_s = 1.0*((I_res_s*3.3)/0.7);   //0.713239227822580 
00289     }
00290     I_msrd_s += G_Cfilter*(I1_act_s-I_msrd_s)*dt;
00291        
00292 }
00293 
00294 //-----------------------=slaveCurrentController=-------------------------------
00295 void slaveCurrentController()
00296 {
00297     I_ref_s = I_sin_gen;           // 2 x PI x t x 
00298     I_error_s = I_ref_s - I_msrd_s;
00299     I_err_sum_s += I_error_s*dt;
00300     I_PID_s = (K_pis*I_error_s)+(K_dis*(I_error_s-I_err_prev_s)/dt) + (K_iis*I_err_sum_s);  
00301     I_err_prev_s = I_error_s; 
00302 }
00303 
00304 //----------------------=Slave Motor run=---------------------------------------
00305 void motorpwm_s(){
00306     duty_s = I_PID_s;
00307      if (duty_s> 0.0)
00308      {
00309         if (duty_s > 0.9)
00310         {
00311             duty_s = 0.9;
00312         }
00313         pwm_clk_s = 0.0;
00314         pwm_anticlk_s = duty_s;
00315     }
00316 
00317     if (duty_s < 0.0)
00318     {
00319         if (duty_s< -0.9)
00320         {
00321             duty_s = -0.9;
00322         }
00323         pwm_anticlk_s = 0.0;
00324         pwm_clk_s = -1.0 * duty_s;
00325     }    
00326 }
00327 
00328 
00329 //--------------------------=Data writting to file=-----------------------------
00330 //--------------------------=Printing to a file=--------------------------------
00331 void thread_2(void const *argument)
00332 {   
00333     while(1)
00334     {
00335        
00336         //pc.printf("%f %f\n",I1_act_m,I_msrd);
00337         //pc.printf("%d\t %f\t %f\t %f\t %f\t\r\n",count,I_cmd,I_ref,I_msrd,x_res);
00338         sd_card_write();
00339     }
00340 }
00341 
00342 
00343 //--------------------------=File print test=-----------------------------------
00344 void sd_card_write_test()
00345 {
00346     mkdir("/sd/mydir", 0777);
00347     
00348     FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
00349     if(fp == NULL) {
00350         error("Could not open file for write\n");
00351     }
00352     fprintf(fp, "count \t x_res\t  RTOB \n");
00353     fclose(fp); 
00354     
00355     //fprintf(fp,"startTime\t I_ref\t I_msrd\t dx_res\t\n");
00356 }
00357 
00358 //--------------------------=File print=----------------------------------------
00359 void sd_card_write()
00360 {
00361     //led1=!led1;
00362     FILE *fp = fopen("/sd/mydir/sdtest.txt","a");
00363     fprintf(fp,"%d\t %f\t  %f\t \r\n",count,I_sin,I_sin_gen);
00364     fclose(fp);
00365 }
00366 
00367 //--------------------------=Ethernet Initialization=---------------------------
00368 
00369 void ethernet_init()
00370 {
00371     eth.set_link(Ethernet::HalfDuplex100);
00372     wait_ms(1000); // Needed after startup.
00373     if(eth.link())
00374     {
00375          for(int i=0;i<3;i++)
00376          { 
00377             led3=!led3;
00378             wait(1.0);
00379          }
00380     }
00381 }