Revision 2:00c6646f42fb, committed 2020-01-10

Comitter:

eembed

Date:

Fri Jan 10 04:08:21 2020 +0000

Parent:

1:db36f62f783b

Commit message:

first commit

Changed in this revision

--- a/SDFileSystem/SDFileSystem.cpp	Mon Aug 19 18:01:21 2019 +0000
+++ b/SDFileSystem/SDFileSystem.cpp	Fri Jan 10 04:08:21 2020 +0000
@@ -207,7 +207,7 @@
         return 1;
     }
     
-    _spi.frequency(1000000); // Set to 1MHz for data transfer
+    _spi.frequency(10000000); // Set to 1MHz for data transfer
     return 0;
 }
 

--- a/main.cpp	Mon Aug 19 18:01:21 2019 +0000
+++ b/main.cpp	Fri Jan 10 04:08:21 2020 +0000
@@ -1,142 +1,203 @@
+/*
+* Force Controlled Vibration Analysis - 
+* 
+* 22.08.2019
+* Theekshana
+*/
+ 
 #include "mbed.h"
 #include "rtos.h"
 #include "SDFileSystem.h"
 #include "qeihw.h"
-//------------------------------------------------------------------------------
-Serial pc(USBTX, USBRX);
+
+//---------------------------=Communication Pins=-------------------------------
+
 DigitalOut led3(LED3);
 DigitalOut led1(LED1);
 SDFileSystem sd(p5, p6, p7, p8, "sd");  // pintout for sd card
-//-------------------------=Thread Variables=-----------------------------------
+Serial pc(USBTX, USBRX);    
+//--------------------------=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(p23);              // clockwise rotation pwm pin for Slave
+PwmOut pwm_anticlk_s(p24);          
+PwmOut pwm_clk(p21);                // clockwise rotation pwm pin for Master
+PwmOut pwm_anticlk(p22); 
+
+//--------------------------=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=-------------------------
+
+//--------------------------=Velocity reading variables=-------------------------
 char buf[0x600];
 float recv_angle = 0.0;
 float x_res = 0.0;
-float dt = 0.000001*150.0;
+float dt = 0.000001*200.0;
 float ve_sum = 0.0;
 float dx_res = 0.0;
 float G_filter_v = 100.0;
 float duty = 0.0;
-//-------------------------=Current PID Variables=------------------------------
-float K_pi=25.0;
-float K_di=0.0;
-float K_ii=1.0;
-float I_ref = 0.02;
-float I_error = 0.0;
-float I_PID = 0.0;
-float I_err_sum = 0.0;
-float I_err_prev = 0.0;
-//--------------------------=Motor Variables=-----------------------------------
-DigitalOut  Reset_AB(p29);
-DigitalOut  Reset_CD(p30);
-PwmOut pwm_clk(p23);          
-PwmOut pwm_anticlk(p24); 
-//-------------------------=DoB variables=--------------------------------------
+
+//--------------------------=DoB variables=--------------------------------------
 float K_tn=0.135;
 float J_n = 0.0000720;
-float g_dis = 100.0;
+float B = 0.0;
+
+float g_dis = 1.0;
 float D_DoB = 0.0;
 float D_BFilter = 0.0;
 float D_AFilter = 0.0;
-//-------------------------=Ethernet Variables=---------------------------------
-Ethernet eth;
+float I_DoB = 0.0;
+
+//--------------------------=RTOB Variables=-------------------------------------
+float RT_BFilter=0.0;
+float RT_AFilter = 0.0;
+float RTOB = 0.0;
+//-------------------------=slave current controller variables=-----------------
+int count = 0;
+int countprev = 0;
+float I_ref = 1.0;
+float K_pis =5.0;
+float K_dis = 0.0;
+float K_iis = 0.0;
 
-//-------------------------=Current controller parameters=----------------------
-int Master_Direction=0;
-DigitalIn M_Dir(p10);
-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;
-//---------------------------------=Velocity=-----------------------------------
+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_res_s  = 0.0;
+float I1_act_s = 0.0;
+float I_msrd_s = 0.0;
+//--------------------------=Velocity controller variables=---------------------
+float V_error=0.0;
+float V_cmd =50.0;
+float k_pv = 0.02;
+float V_pid = 0.0;
+float dx_res_prev = 0.0;
+float dxdx_res = 0.0;
+
+//--------------------------=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();
+    int size2 = eth.receive();
+    if(size2 > 0)
+    {
+        eth.read(buf, size2);
+        memcpy(&recv_angle, buf, sizeof(float));
+        x_res = -1*recv_angle; 
+    } 
+    dx_res_prev = dx_res;
+    ve_sum += dx_res*dt;
+    dx_res =G_filter_v*( x_res-ve_sum);
+    dxdx_res = (dx_res-dx_res_prev)/dt;
+}
+
+
+//--------------------------=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; 
     
-    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.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;
-    }
-    
-//-----------------------------=Current Controller PID=-------------------------
-void currentPID(){
-    I_error = I_ref - I_msrd;
-    I_err_sum+= (I_error*dt);
-    I_PID = (K_pi * I_error)+(K_di*(I_error-I_err_prev)/dt)+(K_ii*I_err_sum);
-    I_err_prev = I_error;
-    }
-//-----------------------------=Motor PWM Initialization=-----------------------
-void motorPWM_init(){
+//--------------------------=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;
-    }
-//-----------------------------=Motor PWM Generation=---------------------------
-void motorPWM(){
-     duty = I_PID;
-     if (duty> 0.0) {
-        if (duty > 0.5) {
-            duty = 0.5;
+    Reset_AB_s =1;
+    Reset_CD_s =1;
+}
+
+//--------------------------=Motor PWM Generation=------------------------------
+void motorPWM()
+{
+     duty = V_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.5) {
-            duty = -0.5;
+    if (duty < 0.0)
+    {
+        if (duty< -0.9)
+        {
+            duty = -0.9;
         }
         pwm_anticlk = 0.0;
         pwm_clk = -1.0 * duty;
     }
-    }
-//--------------------------=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);
-    
+}
+
+//---------------------------=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 
     }
-//--------------------------=Control Loop=--------------------------------------
-void controlLoop(){
-    tempTime = timer.read_us();
-    startTime = tempTime-preTime; 
-    preTime = tempTime;
-    readVelocity(); 
-    readCurrent(); 
-    currentPID();
-    motorPWM();
-    DoB();
-      
+    I_msrd_s += G_Cfilter*(I1_act_s-I_msrd_s)*dt;
+       
 }
-//------------------------------=File print test=-------------------------------
+//--------------------------=File print test=-----------------------------------
 void sd_card_write_test()
 {
     mkdir("/sd/mydir", 0777);
@@ -146,32 +207,22 @@
         error("Could not open file for write\n");
     }
     fprintf(fp, "Hello fun SD Card World!");
-    fclose(fp);
+    //fclose(fp);
     
     //fprintf(fp,"startTime\t I_ref\t I_msrd\t dx_res\t\n");
-
 }
 
-//----------------------------=File print=--------------------------------------
+//--------------------------=File print=----------------------------------------
 void sd_card_write()
 {
-    led1=!led1;
+    //led1=!led1;
     FILE *fp = fopen("/sd/mydir/sdtest.txt","a");
-    fprintf(fp,"%f\t %f\t %f\t %f\t\r\n",startTime, I_ref, I_msrd, dx_res);
+    fprintf(fp,"%f\t \r\n",I_msrd_s);
     fclose(fp);
 }
 
-//------------------------=Printing to a file=----------------------------------
-void thread_2(void const *argument)
-{   
-    while(1)
-    {
-        //pc.printf("%f %f\n",I1_act_m,I_msrd);
-        pc.printf("%f %f %f %f %f %f \n",startTime,I_ref,I_msrd,I_PID,x_res,D_DoB);
-        //sd_card_write();
-    }
-}
-//-----------------------------=Ethernet Initialization=------------------------
+//--------------------------=Ethernet Initialization=---------------------------
+
 void ethernet_init()
 {
     eth.set_link(Ethernet::HalfDuplex100);
@@ -185,14 +236,124 @@
          }
     }
 }
+//--------------------------=velocity controller=-------------------------------
+void velocitycontroller(){
+    V_error=(V_cmd*0.1047198-dx_res);
+    V_pid = k_pv*V_error;
+    
+    }
+//--------------------------=Main Control Loop=---------------------------------
+void controlLoop(){
+    count = count+1;
+    if (count<100000){
+        if (count>1000 and count<1500){
+            V_cmd=100.0;}
+        else if (count>1500 and count<2000){
+            V_cmd=150.0;}
+        else if (count>2000 and count<2500){
+            V_cmd=200.0;}
+        else if (count>2500 and count<3000){
+            V_cmd=250.0;}
+        else if (count>3000 and count<3500){
+            V_cmd=400.0;}
+        else if (count>3500 and count<4000){
+            V_cmd=200.0;}
+        else if (count>4000 and count<4500){
+            V_cmd=400.0;}
+        else if (count>4500 and count<20000){
+            V_cmd=100.0+50.0*sin(0.00314159265*(count-4500));}
+        else if (count>20000 and count<40000){
+            V_cmd=200.0+100.0*sin(0.0062831853*(count-20000));}
+        else if (count>40000 and count<60000){
+            V_cmd=150.0;}
+        else if (count>60000 and count<80000){
+            V_cmd=300.0+150.0*sin(0.0062831853*(count-60000));}
+        else if (count>80000 and count<100000){
+            V_cmd = 250.0;}
+            }
+    else if (count>100000 and count<200000){
+        if (count>100000 and count<100500){
+            V_cmd=150.0;}
+        else if (count>100500 and count<102000){
+            V_cmd=50.0;}
+        else if (count>102000 and count<102500){
+            V_cmd=100.0;}
+        else if (count>102500 and count<103000){
+            V_cmd=200.0;}
+        else if (count>103000 and count<103500){
+            V_cmd=600.0;}
+        else if (count>103500 and count<104000){
+            V_cmd=300.0;}
+        else if (count>104000 and count<104500){
+            V_cmd=250.0;}
+        else if (count>104500 and count<120000){
+            V_cmd=150.0+60.0*sin(0.00314159265*(count-104500));}
+        else if (count>120000 and count<140000){
+            V_cmd=250.0+150.0*sin(0.0062831853*(count-120000));}
+        else if (count>140000 and count<160000){
+            V_cmd=550.0;}
+        else if (count>160000 and count<180000){
+            V_cmd=350.0+200.0*sin(0.0062831853*(count-160000));}
+        else if (count>180000 and count<200000){
+            V_cmd = 500.0;}
+        }
+    else if (count>200000 and count<300000){
+        if (count>200000 and count<200500){
+            V_cmd=70.0;}
+        else if (count>200500 and count<202000){
+            V_cmd=120.0;}
+        else if (count>202000 and count<202500){
+            V_cmd=300.0;}
+        else if (count>202500 and count<203000){
+            V_cmd=100.0;}
+        else if (count>203000 and count<203500){
+            V_cmd=500.0;}
+        else if (count>203500 and count<204000){
+            V_cmd=400.0;}
+        else if (count>204000 and count<204500){
+            V_cmd=250.0;}
+        else if (count>204500 and count<220000){
+            V_cmd=450.0+80.0*sin(0.00314159265*(count-204500));}
+        else if (count>220000 and count<240000){
+            V_cmd=350.0+150.0*sin(0.0062831853*(count-220000));}
+        else if (count>240000 and count<260000){
+            V_cmd=150.0;}
+        else if (count>260000 and count<280000){
+            V_cmd=350.0+200.0*sin(0.0062831853*(count-260000));}
+        else if (count>280000 and count<300000){
+            V_cmd = 400.0;}
+        }
+    slaveCurrentRead();
+    readVelocity(); 
+    velocitycontroller();
+    DoB();
+    motorPWM();
+   
+}
 
 //----------------------------------=Main Loop=---------------------------------
 int main()
 {
-    //sd_card_write_test();
+    
+    sd_card_write_test();
     ethernet_init();
     motorPWM_init();
+    FILE *fp = fopen("/sd/mydir/sdtest.txt","w");
     timer.start();
     time_up.attach(&controlLoop, dt); 
-    Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
-}
\ No newline at end of file
+    //Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
+    while(1)
+    {
+        if (count<3500 and count!=countprev and count%2==0){  //350000
+            led1=0;
+            fprintf(fp,"%d %f %f %f %f\n",count, D_DoB,I_msrd_s,dx_res,dxdx_res); //9.54929658551
+            countprev=count;
+            }
+        else if(count>5000){
+            led1=1;
+            fclose(fp);
+            }
+        //pc.printf("%f\t \r\n",I_msrd_s); //dx_res*9.549296586
+        //sd_card_write();
+    }
+}