File content as of revision 0:5459cdde6298:

#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);
   }