Department of Electrical Eng University of Moratuwa
Force controlled vibration analysis
Revision 4:8432d3d42835, committed 2019-09-06
- Comitter:
- eembed
- Date:
- Fri Sep 06 08:59:53 2019 +0000
- Parent:
- 2:409ee7fcbd8a
- Child:
- 5:9ab19c63203e
- Commit message:
- force control
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Aug 30 12:11:12 2019 +0000
+++ b/main.cpp Fri Sep 06 08:59:53 2019 +0000
@@ -12,7 +12,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
@@ -47,6 +47,10 @@
//--------------------------=Ethernet Variable=---------------------------------
Ethernet eth;
+float I_sin = 0.0;
+float I_DoB_prev = 0.0;
+float I_sin_prev =0.0;
+
//--------------------------=Thread Variables=-----------------------------------
@@ -73,7 +77,7 @@
float B = 0.0;
float g_dis = 100.0;
-float g_Tdis = 70.0;
+float g_Tdis = 75.0;
float D_DoB = 0.0;
float D_BFilter = 0.0;
float D_AFilter = 0.0;
@@ -85,11 +89,20 @@
float RT_AFilter = 0.0;
float RTOB = 0.0;
+float K_pi = 40.0; // tested value for smooth oparation
+float K_di = 0.0;
+float K_ii = 0.0;
+float I_error = 0.0;
+float I_err_sum = 0.0;
+float I_err_prev = 0.0;
+
+float I_PID = 0.0;
+
//--------------------------=Force Controller Variables=-------------------------
float F_ref = 500.0;
-float T_ref = 0.0;
+float T_ref = 0.15;
float T_f = 0.025;
-float K_pf = 25000.0; // tested value for smooth oparation
+float K_pf = 12000.0; // tested value for smooth oparation
float K_df = 0.0;
float K_if = 0.0;
float F_error = 0.0;
@@ -124,6 +137,7 @@
void forceController();
void DoB();
void motorPWM_init();
+
void motorPWM();
void slaveCurrentRead();
@@ -140,12 +154,12 @@
//----------------------------------=Main Loop=---------------------------------
int main()
{
+ ethernet_init();
sd_card_write_test();
FILE *fp = fopen("/sd/mydir/sdtest.txt","w");
- ethernet_init();
motorPWM_init();
time_up.attach(&controlLoop, dt);
- //Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
+
while(1)
{
//pc.printf("%d\t %f\t %f\t %f\t %f\t\r\n",count,T_ref,RTOB,I_msrd,x_res);
@@ -165,16 +179,18 @@
}
count = count+1;
+ I_ref_s = 0.2*sin(2*3.14159*dt*f*count); // 2*3.14159*dt =
readVelocity();
readCurrent();
forceController();
- DoB();
+ DoB();
motorPWM();
slaveCurrentRead();
slaveCurrentController();
motorpwm_s();
}
+
//--------------------------=Read Velocity=-------------------------------------
void readVelocity(){
int size2 = eth.receive();
@@ -196,13 +212,13 @@
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
+ I1_act_m = -1.0*(I_res_m*4.71428); //0.74787687701613 //0.717075441532258
- }else if(Master_Direction == 1) { //master anticlockwise
+ }else { //master anticlockwise
I_res_m = current_sensor_m_n.read();
- I1_act_m = 1.0*((I_res_m*3.3)/0.7); //0.713239227822580
+ I1_act_m = (I_res_m*4.71428); //0.713239227822580
}
- I_msrd += G_Cfilter*(I1_act_m-I_msrd)*dt;
+ I_msrd += 30.0*(I1_act_m-I_msrd)*dt;
}
@@ -210,13 +226,14 @@
//--------------------------=Velocity Controller=-------------------------------
void forceController()
{
- T_ref = F_ref*6.1/10000.0;
+ //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);//+(K_df*(F_error-F_err_prev)/dt) + (K_if*F_err_sum);
F_err_prev = F_error;
I_cmd = F_PID*J_n/K_tn;
- I_ref = I_cmd +I_DoB;
+ I_ref = I_cmd +(I_DoB);
+
}
//--------------------------=Distarbance Observer=------------------------------
@@ -226,6 +243,9 @@
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=------------------------------
R_BFilter=(I_msrd*K_tn) + (dx_res*J_n*g_Tdis);
@@ -258,19 +278,17 @@
duty = I_ref/5.1;
if (duty> 0.0)
{
- if (duty > 1.0)
+ if (duty > 0.95)
{
- duty = 1.0;
+ duty = 0.95;
}
pwm_clk = 0.0;
pwm_anticlk = duty;
- }
-
- if (duty < 0.0)
+ }else
{
- if (duty< -1.0)
+ if (duty< -0.95)
{
- duty = -1.0;
+ duty = -0.95;
}
pwm_anticlk = 0.0;
pwm_clk = -1.0 * duty;
@@ -286,23 +304,22 @@
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
+ I1_act_s = -1.0*((I_res_s*4.71428) );//0.74787687701613 //0.717075441532258
- }else if(slave_Direction == 1) { //master anticlockwise
+ }else { //master anticlockwise
I_res_s = current_sensor_s_n.read();
- I1_act_s = 1.0*((I_res_s*3.3)/0.7); //0.713239227822580
+ I1_act_s = 1.0*((I_res_s*4.71428)); //0.713239227822580
}
- I_msrd_s += G_Cfilter*(I1_act_s-I_msrd_s)*dt;
+ I_msrd_s += 30.0*(I1_act_s-I_msrd_s)*dt; //G_Cfilter = 30.00
}
//-----------------------=slaveCurrentController=-------------------------------
void slaveCurrentController()
-{
- I_ref_s = 0.1*sin(2*3.14159*dt*f*count); // 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_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;
}
@@ -311,19 +328,19 @@
duty_s = I_PID_s;
if (duty_s> 0.0)
{
- if (duty_s > 1.0)
+ if (duty_s > 0.95)
{
- duty_s = 1.0;
+ duty_s = 0.95;
}
pwm_clk_s = 0.0;
pwm_anticlk_s = duty_s;
}
- if (duty_s < 0.0)
+ else
{
- if (duty_s< -1.0)
+ if (duty_s< -0.95)
{
- duty_s = -1.0;
+ duty_s = -0.95;
}
pwm_anticlk_s = 0.0;
pwm_clk_s = -1.0 * duty_s;
@@ -332,17 +349,6 @@
//--------------------------=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,T_ref,RTOB,I_msrd,x_res);
- //sd_card_write();
- }
-}
//--------------------------=File print test=-----------------------------------
@@ -357,7 +363,7 @@
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=----------------------------------------
@@ -367,7 +373,7 @@
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);
+ fprintf(fp,"%d\t %f\t %f\t \r\n",count,x_res,RTOB);
fclose(fp);
}