File content as of revision 1:db36f62f783b:

#include "mbed.h"
#include "rtos.h"
#include "SDFileSystem.h"
#include "qeihw.h"
//------------------------------------------------------------------------------
Serial pc(USBTX, USBRX);
DigitalOut led3(LED3);
DigitalOut led1(LED1);
SDFileSystem sd(p5, p6, p7, p8, "sd");  // pintout for sd card
//-------------------------=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=-------------------------
char buf[0x600];
float recv_angle = 0.0;
float x_res = 0.0;
float dt = 0.000001*150.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=--------------------------------------
float K_tn=0.135;
float J_n = 0.0000720;
float g_dis = 100.0;
float D_DoB = 0.0;
float D_BFilter = 0.0;
float D_AFilter = 0.0;
//-------------------------=Ethernet Variables=---------------------------------
Ethernet eth;

//-------------------------=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=-----------------------------------
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();
    
    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(){
    pwm_clk.period_us(10);
    pwm_anticlk.period_us(10);
    pwm_clk.write(0.0f);       
    pwm_anticlk.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;
        }
        pwm_clk = 0.0;
        pwm_anticlk = duty;
    }

    if (duty < 0.0) {
        if (duty< -0.5) {
            duty = -0.5;
        }
        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);
    
    }
//--------------------------=Control Loop=--------------------------------------
void controlLoop(){
    tempTime = timer.read_us();
    startTime = tempTime-preTime; 
    preTime = tempTime;
    readVelocity(); 
    readCurrent(); 
    currentPID();
    motorPWM();
    DoB();
      
}
//------------------------------=File print test=-------------------------------
void sd_card_write_test()
{
    mkdir("/sd/mydir", 0777);
    
    FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
    if(fp == NULL) {
        error("Could not open file for write\n");
    }
    fprintf(fp, "Hello fun SD Card World!");
    fclose(fp);
    
    //fprintf(fp,"startTime\t I_ref\t I_msrd\t dx_res\t\n");

}

//----------------------------=File print=--------------------------------------
void sd_card_write()
{
    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);
    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=------------------------
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);
         }
    }
}

//----------------------------------=Main Loop=---------------------------------
int main()
{
    //sd_card_write_test();
    ethernet_init();
    motorPWM_init();
    timer.start();
    time_up.attach(&controlLoop, dt); 
    Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
}