File content as of revision 2:00c6646f42fb:

/*
* Force Controlled Vibration Analysis - 
* 
* 22.08.2019
* Theekshana
*/
 
#include "mbed.h"
#include "rtos.h"
#include "SDFileSystem.h"
#include "qeihw.h"

//---------------------------=Communication Pins=-------------------------------

DigitalOut led3(LED3);
DigitalOut led1(LED1);
SDFileSystem sd(p5, p6, p7, p8, "sd");  // pintout for sd card
Serial pc(USBTX, USBRX);    
//--------------------------=Motor Driver Controller Pin =-----------------------------
DigitalOut  Reset_AB(p29);          // H bridge enable
DigitalOut  Reset_CD(p30);
DigitalOut  Reset_AB_s(p27);
DigitalOut  Reset_CD_s(p28);


PwmOut pwm_clk_s(p23);              // clockwise rotation pwm pin for Slave
PwmOut pwm_anticlk_s(p24);          
PwmOut pwm_clk(p21);                // clockwise rotation pwm pin for Master
PwmOut pwm_anticlk(p22); 

//--------------------------=Current Sensor MBED Pins=--------------------------
int Master_Direction=0;
int slave_Direction=0;

DigitalIn M_Dir(p10);
DigitalIn S_Dir(p9);
AnalogIn current_sensor_s_p(p15); // current sensor input for MASTER positive
AnalogIn current_sensor_s_n(p16);
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;

//--------------------------=Ethernet Variable=---------------------------------
Ethernet eth;


//--------------------------=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*200.0;
float ve_sum = 0.0;
float dx_res = 0.0;
float G_filter_v = 100.0;
float duty = 0.0;

//--------------------------=DoB variables=--------------------------------------
float K_tn=0.135;
float J_n = 0.0000720;
float B = 0.0;

float g_dis = 1.0;
float D_DoB = 0.0;
float D_BFilter = 0.0;
float D_AFilter = 0.0;
float I_DoB = 0.0;

//--------------------------=RTOB Variables=-------------------------------------
float RT_BFilter=0.0;
float RT_AFilter = 0.0;
float RTOB = 0.0;
//-------------------------=slave current controller variables=-----------------
int count = 0;
int countprev = 0;
float I_ref = 1.0;
float K_pis =5.0;
float K_dis = 0.0;
float K_iis = 0.0;

float I_error_s = 0.0;
float I_ref_s = 0.0;
float I_err_sum_s = 0.0;
float I_PID_s = 0.0;
float I_err_prev_s = 0.0;
float duty_s = 0.0;

float I_res_s  = 0.0;
float I1_act_s = 0.0;
float I_msrd_s = 0.0;
//--------------------------=Velocity controller variables=---------------------
float V_error=0.0;
float V_cmd =50.0;
float k_pv = 0.02;
float V_pid = 0.0;
float dx_res_prev = 0.0;
float dxdx_res = 0.0;

//--------------------------=Read 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; 
    } 
    dx_res_prev = dx_res;
    ve_sum += dx_res*dt;
    dx_res =G_filter_v*( x_res-ve_sum);
    dxdx_res = (dx_res-dx_res_prev)/dt;
}


//--------------------------=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);
    I_DoB = D_DoB/K_tn; 
    
//--------------------------=Reaction Force Observer=------------------------------  
   /* RT_BFilter = D_BFilter - (T_f+B*dx_res);
    RT_AFilter += g_dis*(RT_BFilter-RT_AFilter)*dt;
    RTOB =  RT_AFilter - (dx_res*J_n*g_dis);*/
}


//--------------------------=Motor PWM Initialization=--------------------------
void motorPWM_init()
{
    pwm_clk.period_us(10);
    pwm_clk_s.period_us(10);
    pwm_anticlk_s.period_us(10);
    pwm_anticlk.period_us(10);
    pwm_clk.write(0.0f);       
    pwm_anticlk.write(0.0f);
    pwm_clk_s.write(0.0f);       
    pwm_anticlk_s.write(0.0f);
    Reset_AB = 1;               
    Reset_CD = 1;
    Reset_AB_s =1;
    Reset_CD_s =1;
}

//--------------------------=Motor PWM Generation=------------------------------
void motorPWM()
{
     duty = V_pid;
     if (duty> 0.0)
     {
        if (duty > 0.9)
        {
            duty = 0.9;
        }
        pwm_clk = 0.0;
        pwm_anticlk = duty;
    }

    if (duty < 0.0)
    {
        if (duty< -0.9)
        {
            duty = -0.9;
        }
        pwm_anticlk = 0.0;
        pwm_clk = -1.0 * duty;
    }
}

//---------------------------=Slave motor current reading=----------------------
void slaveCurrentRead()
{
    slave_Direction = S_Dir.read();
                                                    
    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
         
    }else if(slave_Direction == 1) { //master anticlockwise 
        I_res_s =  current_sensor_s_n.read();
        I1_act_s = 1.0*((I_res_s*3.3)/0.7);   //0.713239227822580 
    }
    I_msrd_s += G_Cfilter*(I1_act_s-I_msrd_s)*dt;
       
}
//--------------------------=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 \r\n",I_msrd_s);
    fclose(fp);
}

//--------------------------=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);
         }
    }
}
//--------------------------=velocity controller=-------------------------------
void velocitycontroller(){
    V_error=(V_cmd*0.1047198-dx_res);
    V_pid = k_pv*V_error;
    
    }
//--------------------------=Main Control Loop=---------------------------------
void controlLoop(){
    count = count+1;
    if (count<100000){
        if (count>1000 and count<1500){
            V_cmd=100.0;}
        else if (count>1500 and count<2000){
            V_cmd=150.0;}
        else if (count>2000 and count<2500){
            V_cmd=200.0;}
        else if (count>2500 and count<3000){
            V_cmd=250.0;}
        else if (count>3000 and count<3500){
            V_cmd=400.0;}
        else if (count>3500 and count<4000){
            V_cmd=200.0;}
        else if (count>4000 and count<4500){
            V_cmd=400.0;}
        else if (count>4500 and count<20000){
            V_cmd=100.0+50.0*sin(0.00314159265*(count-4500));}
        else if (count>20000 and count<40000){
            V_cmd=200.0+100.0*sin(0.0062831853*(count-20000));}
        else if (count>40000 and count<60000){
            V_cmd=150.0;}
        else if (count>60000 and count<80000){
            V_cmd=300.0+150.0*sin(0.0062831853*(count-60000));}
        else if (count>80000 and count<100000){
            V_cmd = 250.0;}
            }
    else if (count>100000 and count<200000){
        if (count>100000 and count<100500){
            V_cmd=150.0;}
        else if (count>100500 and count<102000){
            V_cmd=50.0;}
        else if (count>102000 and count<102500){
            V_cmd=100.0;}
        else if (count>102500 and count<103000){
            V_cmd=200.0;}
        else if (count>103000 and count<103500){
            V_cmd=600.0;}
        else if (count>103500 and count<104000){
            V_cmd=300.0;}
        else if (count>104000 and count<104500){
            V_cmd=250.0;}
        else if (count>104500 and count<120000){
            V_cmd=150.0+60.0*sin(0.00314159265*(count-104500));}
        else if (count>120000 and count<140000){
            V_cmd=250.0+150.0*sin(0.0062831853*(count-120000));}
        else if (count>140000 and count<160000){
            V_cmd=550.0;}
        else if (count>160000 and count<180000){
            V_cmd=350.0+200.0*sin(0.0062831853*(count-160000));}
        else if (count>180000 and count<200000){
            V_cmd = 500.0;}
        }
    else if (count>200000 and count<300000){
        if (count>200000 and count<200500){
            V_cmd=70.0;}
        else if (count>200500 and count<202000){
            V_cmd=120.0;}
        else if (count>202000 and count<202500){
            V_cmd=300.0;}
        else if (count>202500 and count<203000){
            V_cmd=100.0;}
        else if (count>203000 and count<203500){
            V_cmd=500.0;}
        else if (count>203500 and count<204000){
            V_cmd=400.0;}
        else if (count>204000 and count<204500){
            V_cmd=250.0;}
        else if (count>204500 and count<220000){
            V_cmd=450.0+80.0*sin(0.00314159265*(count-204500));}
        else if (count>220000 and count<240000){
            V_cmd=350.0+150.0*sin(0.0062831853*(count-220000));}
        else if (count>240000 and count<260000){
            V_cmd=150.0;}
        else if (count>260000 and count<280000){
            V_cmd=350.0+200.0*sin(0.0062831853*(count-260000));}
        else if (count>280000 and count<300000){
            V_cmd = 400.0;}
        }
    slaveCurrentRead();
    readVelocity(); 
    velocitycontroller();
    DoB();
    motorPWM();
   
}

//----------------------------------=Main Loop=---------------------------------
int main()
{
    
    sd_card_write_test();
    ethernet_init();
    motorPWM_init();
    FILE *fp = fopen("/sd/mydir/sdtest.txt","w");
    timer.start();
    time_up.attach(&controlLoop, dt); 
    //Thread thread(*thread_2,NULL,osPriorityAboveNormal,DEFAULT_STACK_SIZE*10.0,NULL);
    while(1)
    {
        if (count<3500 and count!=countprev and count%2==0){  //350000
            led1=0;
            fprintf(fp,"%d %f %f %f %f\n",count, D_DoB,I_msrd_s,dx_res,dxdx_res); //9.54929658551
            countprev=count;
            }
        else if(count>5000){
            led1=1;
            fclose(fp);
            }
        //pc.printf("%f\t \r\n",I_msrd_s); //dx_res*9.549296586
        //sd_card_write();
    }
}