Revision 5:9ab19c63203e, committed 2019-12-03

Comitter:

eembed

Date:

Tue Dec 03 10:40:16 2019 +0000

Parent:

4:8432d3d42835

Commit message:

first commit;

Changed in this revision

--- a/SDFileSystem/SDFileSystem.cpp	Fri Sep 06 08:59:53 2019 +0000
+++ b/SDFileSystem/SDFileSystem.cpp	Tue Dec 03 10:40:16 2019 +0000
@@ -141,7 +141,7 @@
 
 int SDFileSystem::initialise_card() {
     // Set to 100kHz for initialisation, and clock card with cs = 1
-    _spi.frequency(100000);
+    _spi.frequency(1000000);
     _cs = 1;
     for (int i = 0; i < 16; i++) {
         _spi.write(0xFF);

--- a/main.cpp	Fri Sep 06 08:59:53 2019 +0000
+++ b/main.cpp	Tue Dec 03 10:40:16 2019 +0000
@@ -1,3 +1,4 @@
+
 /*
 * Force Controlled Vibration Analysis - 
 * 
@@ -12,7 +13,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
@@ -65,7 +66,7 @@
 char buf[0x600];
 float recv_angle = 0.0;
 float x_res = 0.0;
-float dt = 0.000001*200.0;
+float dt = 0.000001*150.0;
 float ve_sum = 0.0;
 float dx_res = 0.0;
 float G_filter_v = 150.0;
@@ -74,10 +75,10 @@
 //--------------------------=DoB variables=--------------------------------------
 float K_tn=0.135;
 float J_n = 0.0000720;
-float B = 0.0;
+float B = 0.000107;
 
 float g_dis = 100.0;
-float g_Tdis = 75.0;
+float g_Tdis = 5.0;
 float D_DoB = 0.0;
 float D_BFilter = 0.0;
 float D_AFilter = 0.0;
@@ -102,7 +103,7 @@
 float F_ref = 500.0;
 float T_ref = 0.15;
 float T_f = 0.025;
-float K_pf = 12000.0;                       // tested value for smooth oparation
+float K_pf = 10000.0;                       // tested value for smooth oparation
 float K_df = 0.0;
 float K_if = 0.0;
 float F_error = 0.0;
@@ -110,7 +111,7 @@
 float F_err_prev = 0.0;
 float I_cmd = 0.0;
 float F_PID = 0.0;
-float I_ref = 0.0;
+float I_ref = 0.5;
 
 //-------------------------=slave current controller variables=-----------------
 int count = 0;
@@ -149,21 +150,36 @@
 void sd_card_write();
 void ethernet_init();
 
+//FILE *fp = fopen("/sd/mydir/sdtest.txt","w");
+
+
 
 
 //----------------------------------=Main Loop=---------------------------------
 int main()
 {
     ethernet_init();
-    sd_card_write_test();
-    FILE *fp = fopen("/sd/mydir/sdtest.txt","w");
+    //sd_card_write_test();
+    //FILE *fp = fopen("/sd/mydir/sdtest.txt","w");
     motorPWM_init();
     time_up.attach(&controlLoop, dt); 
   
      while(1)
     {
-               //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();
+        pc.printf("%f\t %f\t %f\t \r\n", I_msrd,I_ref,RTOB);
+        /*
+    if (count>20000 and count<50000 and count!=count_prev ){
+               led1 =1;
+               //sd_card_write();
+               count_prev = count; 
+               //datacount++;              
+               }
+    else if(count>50000){
+             led1=0;
+             fclose(fp);  
+               }
+               
+       */
     }
 }
 
@@ -179,11 +195,11 @@
     }
     
     count = count+1;
-    I_ref_s = 0.2*sin(2*3.14159*dt*f*count);            // 2*3.14159*dt = 
+    I_ref_s = 0.15*sin(2*3.14159*dt*f*count);            // 2*3.14159*dt = 
     readVelocity(); 
     readCurrent();
     forceController();
-     DoB();
+   DoB();
     motorPWM();
     slaveCurrentRead();
     slaveCurrentController();
@@ -229,7 +245,7 @@
     //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);
     F_err_prev = F_error;
     I_cmd = F_PID*J_n/K_tn;
     I_ref = I_cmd +(I_DoB);  
@@ -239,13 +255,12 @@
 //--------------------------=Distarbance Observer=------------------------------
 void DoB()
 {
-    D_BFilter=(I_msrd*K_tn) +  (dx_res*J_n*g_dis);
+    //D_BFilter=(I_msrd*K_tn) +  (dx_res*J_n*g_dis);
+    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;*/
+    I_DoB = (D_DoB)/K_tn; 
+
     
 //--------------------------=Reaction Force Observer=------------------------------  
     R_BFilter=(I_msrd*K_tn) +  (dx_res*J_n*g_Tdis);
@@ -275,7 +290,9 @@
 //--------------------------=Motor PWM Generation=------------------------------
 void motorPWM()
 {
-     duty = I_ref/5.1;
+    duty = I_ref/5.1;
+     //duty = 1.62/5.1;
+      
      if (duty> 0.0)
      {
         if (duty > 0.95)
@@ -319,7 +336,7 @@
 {    
     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);
     I_err_prev_s = I_error_s; 
 }
 
@@ -370,11 +387,12 @@
 void sd_card_write()
 {
     //sprintf (str, "%d\n",count);
-    FILE *fp = fopen("/sd/mydir/sdtest.txt","a");
+    //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);    
-    fclose(fp);
+    //xres = ceil(x_res*1000);
+    //fprintf(fp,"%d\t %f\t %f\t \r\n",count,I_DoB,RTOB);    
+    //fclose(fp);
 }
 
 //--------------------------=Ethernet Initialization=---------------------------