Diff: main.cpp

Revision:

0:5459cdde6298

Child:

1:db36f62f783b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 01 08:00:33 2019 +0000
@@ -0,0 +1,411 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "SDFileSystem.h"
+#include "qeihw.h"
+
+#define Kp 80000.0          //8000.0    //max 100,000   //7000  //100000
+#define Kv 50.0             //50.0      //500.0         //500
+#define Gd 300.0            //200
+#define PI 3.141592653
+
+QEIHW qei_s(QEI_DIRINV_NONE, QEI_SIGNALMODE_QUAD, QEI_CAPMODE_4X, QEI_INVINX_NONE );
+
+//LocalFileSystem local("local"); 
+int position_m = 0; 
+int position = 0; 
+int position_s = 0; 
+float encoder_m_prv=0.0,encoder_s_prv=0.0;
+
+float const G_filcon_I1_m = 350.0; 
+float const G_filcon_I1_s = 350.0; 
+float  I1_act_m=0.0;
+float  I1_act_s=0.0;
+
+// Mutex stdio_mutex;
+// Configuring two encoder modules
+void ethernet_init();
+Ethernet eth;
+
+//Variable for get angle from ethernet
+char buf[0x600];
+float recv_m_angle = 0.0;
+float recv_s_angle = 0.0;
+float inc_now = 0.0, inc_pre = 0.0; 
+
+// Safety for mbed unused pins
+DigitalIn safety_19(p19);
+DigitalIn safety_20(p20);
+DigitalIn safety_25(p25);
+DigitalIn safety_26(p26);
+
+PwmOut pwm_m_clk(p21);          // clockwise rotation pwm pin for MASTER
+PwmOut pwm_m_anticlk(p22);      // anti - clockwise rotation pwm pin for MASTER
+PwmOut pwm_s_clk(p23);          // clockwise rotation pwm pin for SLAVE
+PwmOut pwm_s_anticlk(p24);      // anti-clockwise rotation pwm pin for SLAVE
+
+DigitalOut  Reset_AB_M(p27);
+DigitalOut  Reset_CD_M(p28);
+DigitalOut  Reset_AB_S(p29);
+DigitalOut  Reset_CD_S(p30);
+
+DigitalIn M_Dir(p9);
+DigitalIn S_Dir(p10);
+AnalogIn current_sensor_m_p(p15); // current sensor input for MASTER positive
+AnalogIn current_sensor_m_n(p16); // current sensor input for MASTER negative
+AnalogIn current_sensor_s_p(p17); // current sensor input for SLAVE positive
+AnalogIn current_sensor_s_n(p18); // current sensor input for SLAVE negative
+
+DigitalOut led1(LED1);
+
+DigitalOut led3(LED3);
+
+Timer timer;                    // For the controller
+FILE *fp;
+Ticker ticker;
+//Mutex stdio_mutex; 
+
+int dt_us= 150, ramp_time=0.0;                 // define main loop time in us
+float dt;      //loop time in seconds for calculations     
+
+int counter_time;
+int counter=0;
+int counter_old=0; 
+
+//Current Sensor Directions
+int Master_Direction=0;
+int Slave_Direction = 0;
+
+// Encoder Constants
+float const encoder_pulses_s = 2400.0;
+
+// PID parameters for Current - Loop
+
+//float const Ikp_m = 1.0, Iki_m =2.0, Ikd_m = 0.01;      
+//float const Ikp_m = 0.01/*0.09*/, Iki_m =0.00, Ikd_m = 0.001;  
+//float const Ikp_s = 0.04/*4.5*/, Iki_s = 0.00, Ikd_s = 0.01;
+float const Ikp_m = 0.026/*0.2*/, Iki_m =0.00, Ikd_m = 0.0001;  //j=0.0000720,kp=0.026
+float const Ikp_s = 0.026/*0.2*/, Iki_s = 0.00, Ikd_s = 0.0001;
+
+
+float I_ref_m = 0.0, I_err_m = 0.0, I_res_m = 0.0, I_tmp_m = 0.0, tem_I_m = 0.0, d_I_m = 0.0,I_ref_m1 = 0.0;
+float I_ref_s = 0.0, I_err_s = 0.0, I_res_s = 0.0, I_tmp_s = 0.0, tem_I_s = 0.0, d_I_s = 0.0,I_ref_s1 = 0.0;
+//some may not use..
+float v_ref_m = 00.0, v_err_m = 0.0, v_res_m = 0.0,v_res_m_rpm = 0.0, v_tmp_m = 0.0, tem_v_m = 0.0, d_v_m = 0.0;
+float v_ref_s = 00.0, v_err_s = 0.0, v_res_s = 0.0,v_res_s_rpm = 0.0, v_tmp_s = 0.0, tem_v_s = 0.0, d_v_s = 0.0;
+
+float De_m=0.0, dx_res_m = 0.0, prev_v_err_m=0.0,I_v_err_m=0.0,de_m =0.0;
+float De_s=0.0, dx_res_s = 0.0, prev_v_err_s=0.0,I_v_err_s=0.0,de_s =0.0;
+
+float duty_m = 0.0;                 // PWM duty for master
+float duty_s = 0.0;
+
+float const G_filcon_I_m = 100.0;   // Low pass filter gain for Current - loop default 1
+float const G_filcon_I_s = 100.0;
+
+float const G_filcon_v_m = 100.0;     // Low pass filter gain Velocity estimation
+float const G_filcon_v_s = 100.0;
+
+float I_act_m = 0.0;                // Storing actual current flow
+float I_act_s = 0.0;
+
+float x_res_m_prv = 0.0;      // Parameters to calculate current rotational speed
+float x_res_s_prv = 0.0;
+float encoder_m_now = 0.0;
+float encoder_s_now = 0.0;
+
+// Motor Constant and Inertia
+float const J_const_m = 0.0000720;//0.0000010;// //0.0000800;//0.000072
+float const J_const_s = 0.0000720;//0.0000010;// //0.0000800;
+float const Kt_const_m = 0.135;
+float const Kt_const_s = 0.135;//0.134
+float const Kt_constinv_m = 7.407;//1.43;
+float const Kt_constinv_s = 7.463;//1.43;    
+
+float ddx_ref_m=0.0, ddx_ref_s=0.0;
+                          
+float tmp_m = 0.0,ob_sum_m = 0.0;
+float tmp_s = 0.0,ob_sum_s = 0.0;
+float i_com_m = 0.0;
+float i_com_s = 0.0;
+float fric_m = 0.0,fric_s = 0.0,i_rto_m = 0.0,i_rto_s = 0.0;
+
+float x_res_m = 0.0;
+float x_res_s = 0.0;
+float ve_sum_m = 0.0;
+float ve_sum_s = 0.0;
+float pwm_I_M= 0.0;
+float pwm_I_S= 0.0;
+float pid_V_I_M= 0.0;
+float pid_V_I_S= 0.0;
+
+
+float temp_a=0.0;
+float temp_b=0.0;
+float temp_a2=0.0;
+float temp_b2=0.0;
+float g_a1=1.0;
+float g_b1=1.4;
+float g_b2=1.4;
+float x_res_s_filter=0.0;
+float x_res_m_filter=0.0;
+float dx_res_s_filter=0.0;
+float dx_res_m_filter=0.0;
+float ob_sum_m1=0.0;
+float ob_sum_s1=0.0,torque_dob_m=0.0,torque_dob_s=0.0;
+//float error=0.0;
+
+ void pwm_init(void) {
+
+    pwm_m_clk.period_us(10);
+    pwm_m_anticlk.period_us(10);
+    pwm_s_anticlk.period_us(10);
+    pwm_s_clk.period_us(10);
+
+    pwm_m_clk.write(0.0f);         // Set the ouput duty-cycle, specified as a percentage (float)
+    pwm_m_anticlk.write(0.0f);
+    pwm_s_anticlk.write(0.0f);
+    pwm_s_clk.write(0.0f);
+    
+    Reset_AB_M = 1;               //ENABLE RUNNING MODE  (H BRIDGE ENABLE)
+    Reset_CD_M = 1;
+    Reset_AB_S = 1;
+    Reset_CD_S = 1; 
+}
+
+void velocity_m() {
+        
+        int size2 = eth.receive();
+     
+        if(size2 > 0) {
+        eth.read(buf, size2);
+        memcpy(&recv_m_angle, buf, sizeof(float));
+        x_res_m = recv_m_angle; 
+        }
+                
+     ve_sum_m += dx_res_m*dt;
+     dx_res_m =G_filcon_v_m*( x_res_m-ve_sum_m);      
+           
+}
+
+void velocity_s() {
+
+    int32_t temp; position = 0; 
+    float d_v_err_s=0.0;                     
+    qei_s.SetDigiFilter(480UL);
+    qei_s.SetMaxPosition(0xFFFFFFFF);
+    position = qei_s.GetPosition();
+ 
+    x_res_s = -1.0*position * 2.0 * PI / encoder_pulses_s;
+    ve_sum_s += dx_res_s*dt;
+    dx_res_s = G_filcon_v_s*(x_res_s-ve_sum_s);  
+ 
+}
+
+void current_pid_m(){
+   Master_Direction = M_Dir.read();
+   
+    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.717075441532258) );  //0.74787687701613
+         
+    }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.717075441532258);    //0.713239227822580    
+    }
+   //I_act_m += G_filcon_I1_m*(I1_act_m-I_act_m)*dt;
+   I_act_m = 1*I1_act_m;
+
+    I_err_m = I_ref_m - I_act_m;
+    I_tmp_m += (Iki_m * dt * I_err_m);
+   
+    tem_I_m += G_filcon_I_m*d_I_m*dt;
+    d_I_m = I_err_m - tem_I_m;
+ 
+    pwm_I_M=((I_err_m * Ikp_m) + I_tmp_m + (d_I_m * Ikd_m));
+ }                              
+
+void current_pid_s(){
+    Slave_Direction = S_Dir.read();
+    if(Slave_Direction == 0){                                             //slave clockwise  
+        I_res_s =  current_sensor_s_p.read();
+        I1_act_s = 1.0*((I_res_s*3.3)/0.717075441532258 );    //0.717075441532258
+    }else{                                     
+        I_res_s =  current_sensor_s_n.read();                   //slave anticlockwise
+        I1_act_s = -1.0*((I_res_s*3.3)/0.717075441532258 );     //0.724138445564516
+    }
+    
+  //I_act_s += G_filcon_I1_s*(I1_act_s-I_act_s)*dt;
+    I_act_s = I1_act_s;
+  
+    I_err_s = I_ref_s - I_act_s;
+    I_tmp_s += Iki_s * dt * I_err_s;
+    tem_I_s += G_filcon_I_s*d_I_s*dt;
+    d_I_s = I_err_s - tem_I_s;
+
+   pwm_I_S=((I_err_s * Ikp_s) + I_tmp_s + (d_I_s * Ikd_s));
+}
+
+void Disob() {
+
+    tmp_m = Gd*J_const_s*dx_res_m;            
+    //ob_sum_m  += (Gd*(Kt_const_s*I_act_m+tmp_m-ob_sum_m)*dt);
+    ob_sum_m1  = Kt_const_m*I_act_m+tmp_m;
+    ob_sum_m  += Gd*(ob_sum_m1-ob_sum_m)*dt;
+    i_com_m  = (ob_sum_m - tmp_m)*Kt_constinv_m; //read current
+    //i_com_m=0.0;
+    torque_dob_m= i_com_m*Kt_const_m;
+    
+    fric_m = 0.0;
+    i_rto_m = (i_com_m-fric_m); //friction should be in current terms
+    
+    tmp_s = Gd*J_const_s*dx_res_s; 
+    //ob_sum_s += (Gd*(Kt_const_s*I_act_s+tmp_s-ob_sum_s)*dt);
+    ob_sum_s1 = Kt_const_s*I_act_s+tmp_s;
+    ob_sum_s += Gd*(ob_sum_s1-ob_sum_s)*dt;
+    i_com_s  = (ob_sum_s - tmp_s)*Kt_constinv_s;            //read current
+    //i_com_s = 0.0;
+    torque_dob_s = i_com_s*Kt_const_s;
+    fric_s = 0.0;
+    i_rto_s = (i_com_s-fric_s); //friction should be in current terms
+}
+
+int Controller(void) {
+    
+    ddx_ref_m = Kp*(x_res_s-x_res_m) +  Kv*(dx_res_s-dx_res_m);
+    I_ref_m1 = Kt_constinv_m*J_const_m*ddx_ref_m;
+    I_ref_m  = I_ref_m1 + i_com_m - (i_rto_m+i_rto_s);
+    
+    //I_ref_m = 0.1;
+    ddx_ref_s = Kp*(x_res_m-x_res_s) +  Kv*(dx_res_m-dx_res_s);
+    
+    I_ref_s1 = Kt_constinv_s*J_const_s*ddx_ref_s;
+    I_ref_s  =I_ref_s1 + i_com_s - (i_rto_s+i_rto_m);
+   
+    //I_ref_s = 0.1;
+    
+    return 0;
+}
+
+void PWM_Generator_m() {
+      duty_m =  pwm_I_M;   
+      
+    if (duty_m> 0.0) {
+        if (duty_m > 0.5) {
+            duty_m = 0.5;
+        }
+        pwm_m_clk = 0.0;
+        pwm_m_anticlk = duty_m;
+    }
+
+    if (duty_m < 0.0) {
+        if (duty_m < -0.5) {
+            duty_m = -0.5;
+        }
+        pwm_m_anticlk = 0.0;
+        pwm_m_clk = -1.0 * duty_m;
+    }
+}
+
+void PWM_Generator_s() {
+      duty_s = pwm_I_S;   
+ 
+    if (duty_s > 0.0) {
+        if (duty_s > 0.5) {
+            duty_s = 0.5;
+        }
+        pwm_s_clk = 0.0;
+        pwm_s_anticlk = duty_s;
+    }
+
+    if (duty_s < 0.0) {
+        if (duty_s < -0.5) {
+            duty_s = -0.5;
+        }
+        pwm_s_anticlk = 0.0;
+        pwm_s_clk = -1.0 * duty_s;
+    }
+}
+
+void cleanup_module(void){ 
+             
+    pwm_m_clk = 0.0;         // pwm cleanup module
+    pwm_m_anticlk = 0.0;
+    pwm_s_anticlk = 0.0;
+    pwm_s_clk = 0.0;
+    
+    Reset_AB_M = 0;      //RESET H BRIDGE
+    Reset_CD_M = 0;
+    Reset_AB_S = 0;
+    Reset_CD_S = 0;  
+    
+    led1=0;  
+    led3=0;      
+}
+
+//RTOS
+
+void Control_body() {                                                           
+                               // Control Part - main code       
+        Controller();
+        velocity_m();
+        velocity_s ();          
+        current_pid_m(); 
+        current_pid_s ();    
+        PWM_Generator_m();                
+        PWM_Generator_s(); 
+        Disob();     
+        //led1=!led1;  
+         counter++;                                
+   }
+
+void thread_2(void const *argument){
+ led1=1;
+        SDFileSystem sd(p5, p6, p7, p8, "sd");
+        FILE *fp = fopen("/sd/BC.txt", "w");   
+            
+ if(fp == NULL) {
+         for(int i=0;i<5;i++){ 
+        led3=!led3;
+         wait(1.0);
+            }
+         }
+         
+   while(counter<80000){
+    if(counter>=(counter_old+100)){
+        //fprintf(fp, "%d %f %f %f %f %f %f %f %f   \n",timer.read_us(), x_res_m,x_res_s,I_act_m,I_act_s,torque_dob_s,torque_dob_m,i_com_s,i_com_m); 
+        fprintf(fp, "%d %f %f %f %f \n",timer.read_us(),torque_dob_m,torque_dob_s,x_res_m,x_res_s); 
+        counter_old=counter;
+        led3=!led3; 
+           }            
+        }
+                      
+      fclose(fp); 
+      timer.stop();
+      cleanup_module();             
+      ticker.detach ();     
+    
+      wait(1.0);             
+   }  
+   
+ 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);
+         }
+    }
+}   
+    
+int main() {      
+
+    ethernet_init();  
+    pwm_init();
+    timer.start();
+    dt=dt_us/1000000.0;
+
+    ticker.attach_us(&Control_body, dt_us);  
+    Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
+   }       
+  
\ No newline at end of file