File content as of revision 17:10be89ff33e8:

//  DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase.
//      ver. 121208c by Kosaka lab.
#include "mbed.h"
#include "rtos.h"
#include "QEI.h"
#define PI 3.14159265358979 // def. of PI
/*********** User setting for control parameters (begin) ***************/
//#define SIMULATION          // Comment this line if not simulation
#define USE_PWM             // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0. Comment if use Vref=analog mode
  #define PWM_FREQ 10000.0  //[Hz], pwm freq. available if USE_PWM is defined.
//#define USE_CURRENT_CONTROL // Current control on. Comment if current control off.
#define CONTROL_MODE    0   // 0:PID control, 1:Frequency response, 2:Step response, 3. u=Rand to identify G(s), 4) FFT identification
#define DEADZONE_PLUS   1.  // deadzone of plus side
#define DEADZONE_MINUS -1.5 // deadzone of minus side
#define GOOD_DATA           // Comment this line if the length of data TMAX/TS2 > 1000
//#define R_SIN               // Comment this line if r=step, not r = sin
float   _freq_u = 0.3;      // [Hz], freq. of Frequency response, or Step response
float   _rmax=360./180.*PI; // [rad], max. of reference signal
float   _Kp4th=20;             // P gain for PID from motor volt. to angle.
float   _Ki4th=20;             // I gain for PID from motor volt. to angle.
float   _Kd4th=5;              // D gain for PID from motor volt. to angle.
float   _Kp4i=10.0;            // P gain for PID from motor volt. to motor current.
float   _Ki4i=10.0;            // I gain for PID from motor volt. to motor current.
float   _Kd4i=0.0;             // D gain for PID from motor volt. to motor current.
#define TS0     0.001//08//8      // [s], sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt
#define TS1     0.002//2//0.01       // [s], sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer
#define TS2     0.05        // [s], sampling time (priority =main(): precision 4ms)
#define TS3     0.02        // [s], sampling time (priority low: precision 4ms) to save data to PC using thread. But, max data length is 1000.
#define TS4     0.2         // [s], sampling time (priority lowest: precision 4ms) to display data to PC tera term
#define TMAX    10          // [s], experiment starts from 0[s] to TMAX[s]
#define UMAX    3.3         // [V], max of control input u
#define UMIN   -3.3         // [V], max of control input u
#define IMAX    0.5         // [A], max of motor current i
#define IMIN   -0.5         // [A], max of motor current i
#define DEADTIME    0.0001  // [s], deadtime to be set between plus volt. to/from minus
                        // H bridge port setting
#define FIN_PORT    p21     // FIN (IN1) port of mbed
#define RIN_PORT    p22     // RIN (IN2) port of mbed
#define VREF_PORT   p18     // Vref      port of mbed (available if USE_PWM is not defined)
DigitalOut  debug_p17(p17); // p17 for debug
DigitalOut  debug_p23(p23); // p17 for debug
DigitalOut  debug_p24(p24); // p17 for debug
AnalogIn v_shunt_r(p19);    // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
#define R_SHUNT     1.25    // [Ohm], shunt resistanse
//AnalogIn VCC(p19);          // *3.3 [V], Volt of VCC for motor
//AnalogIn VCC2(p20);         // *3.3 [V], Volt of (VCC-R i), R=2.5[Ohm]. R is for preventing too much i when deadtime is failed.

#define N_ENC   (24*4)     // "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder.
QEI encoder (p29, p30, NC, N_ENC, QEI::X4_ENCODING);
//  QEI(PinName     channelA, mbed pin for channel A input.
//      PinName     channelB, mbed pin for channel B input.
//      PinName     index,    mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed).
//      int         pulsesPerRev, Number of pulses in one revolution(=360 deg).
//      Encoding    encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as 
//                    X4 uses them on both channels.
//  )
//  void     reset (void)
//     Reset the encoder. 
//  int      getCurrentState (void)
//     Read the state of the encoder. 
//  int      getPulses (void)
//     Read the number of pulses recorded by the encoder. 
//  int      getRevolutions (void)
//     Read the number of revolutions recorded by the encoder on the index channel. 
/*********** User setting for control parameters (end) ***************/


Serial pc(USBTX, USBRX);        // Display on tera term in PC 
LocalFileSystem local("local"); // save data to mbed USB disk drive in PC
//Semaphore semaphore1(1);      // wait and release to protect memories and so on
//Mutex stdio_mutex;            // wait and release to protect memories and so on
Ticker TickerTimerTS0;          // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer.

#ifdef  USE_PWM             // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0.
  PwmOut FIN(FIN_PORT);     // PWM for FIN, RIN=0 when forward rotation. H bridge driver PWM mode
  PwmOut RIN(RIN_PORT);     // PWM for RIN, FIN=0 when reverse rotation. H bridge driver PWM mode
#else                       // H bridge Vref=analog mode
  DigitalOut  FIN(FIN_PORT);// FIN  for DC motor H bridge driver. FIN=1, RIN=0 then forward rotation
  DigitalOut  RIN(RIN_PORT);// RIN  for DC motor H bridge driver. FIN=0, RIN=1 then reverse rotation
#endif
AnalogOut   analog_out(VREF_PORT);// Vref for DC motor H bridge driver. DA converter for control input [0.0-1.0]% in the output range of 0.0 to 3.3[V]

unsigned long _count;   // sampling number
float   _time;          // time[s]
float   _r;             // reference signal
float   _th=0;          // [rad], motor angle, control output of angle controller
float   _i=0;           // [A], motor current, control output of current controller
float   _e=0;           // e=r-y for PID controller
float   _eI=0;          // integral of e for PID controller
float   _iref;          // reference current iref [A], output of angle th_contorller
float   _u;             // control input[V], motor input volt.
float   _ei=0;          // e=r-y for current PID controller
float   _eiI=0;         // integral of e for current PID controller
unsigned char _f_u_plus=1;// sign(u)
unsigned char _f_umax=0;// flag showing u is max or not
unsigned char _f_imax=0;// flag showing i is max or not
float   debug[20];      // for debug
float   disp[10];       // for printf to avoid interrupted by quicker process
#ifdef  GOOD_DATA
float data[1000][5];    // memory to save data offline instead of "online fprintf".
unsigned int    count3; // 
unsigned int    count2=(int)(TS3/TS0); // 
unsigned int    _count_data=0; // data2mbedUSB()
#endif

extern "C" void mbed_reset();

void u2Hbridge(float u){// input u to H bridge driver
    float           duty;
    unsigned int    f_deadtime, f_in, r_in;

//    u=-u;   // Set PWM Minus logic for smooth shunt current
    if( u > 0 ){        // forward: rotate to plus
        u += DEADZONE_PLUS; // deadzone compensation
        duty = u/3.3;       // Vref
        if(_f_u_plus==0){   // if plus to/from minus, set FIN=RIN=0/1 for deadtime 100[us].
            f_deadtime = 1; // deadtime is required
            _f_u_plus=1;
        }else{
            f_deadtime = 0; // deadtime is required
        }
        f_in=1;  r_in=0;    // set forward direction
    }else if( u < 0 ){  // reverse: rotate to minus
        u += DEADZONE_MINUS;// deadzone compensation
        duty = -u/3.3;
        if(_f_u_plus==1){   // if plus to/from minus, set FIN=RIN=0/1 for deadtime 100[us].
            f_deadtime = 1; // deadtime is required
            _f_u_plus=0;
        }else{
            f_deadtime = 0; // deadtime is required
        }
        f_in=0;  r_in=1;    // set reverse direction
    }else{// if( u == 0 ){  // stop mode
        duty = 0;
        f_deadtime = 0; // deadtime is required
        f_in=0;  r_in=0;    // set FIN & RIN
    }

    if( f_deadtime==1 ){// making deadtime
        FIN=0;  RIN=0;      // set upper&lower arm zero
        wait(DEADTIME);
    }
#ifdef USE_PWM  // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0
    FIN = duty*(float)f_in;    RIN = duty*(float)r_in;  // setting pwm FIN & RIN
//    FIN = (1-duty*(float)f_in);    RIN = (1-duty*(float)r_in);  // setting pwm FIN & RIN. Minus logic for smooth shunt current
    analog_out = 1;          // setting Vref=UMAX, but Vref=Vcc is better.
#else           //       Analog mode: Vref=analog, FIN, RIN = 1 or 0)
    FIN = f_in;    RIN = r_in;  // setting FIN & RIN
    analog_out = duty;          // setting Vref : PID write DA, range is 0-1. Output voltage 0-3.3v
#endif
}

void th_controller(void const *argument) {    // if rtos. current controller & velocity controller
    float   e_old, wt;
    float   y, u;

//    y_old = _th;  // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data
#ifdef SIMULATION
    if( (u=_iref)>IMAX ){  u-=IMAX;}else if(u<IMIN){    u+=IMIN;}
    y = _th + TS1/10*(20*u-_th);   //=(1-TS/0.1)*_y + 0.2*TS/0.1*_iref; // G = 20/(10s+1)
debug[0] =_iref;
#else
//    semaphore1.wait();      //
    y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
//    semaphore1.release();   //
#endif
#define RMIN  0
    wt = _freq_u *2.0*PI*_time;
    if(wt>2.0*PI){    wt -= 2.0*PI*(float)((int)(wt/(2.0*PI)));}
    _r = sin(wt ) * (_rmax-RMIN)/2.0 + (_rmax+RMIN)/2.0;
#ifndef R_SIN
    if( _r>=(_rmax+RMIN)/2.0 ) _r = _rmax;
    else        _r = 0;
#endif
    e_old = _e;     // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
    _e = _r - y;    // error e(t)
//debug[0]=_e;
    if( _e<((360.0/N_ENC)/180*PI) && _e>-((360.0/N_ENC)/180*PI) ){   // e is inside minimum precision?
        _e = 0;
    }
    if( _f_imax==0 ){   // u is saturated?
//        if( _e>((360.0/N_ENC)/180*PI) || _e<-((360.0/N_ENC)/180*PI) ){   // e is inside minimum precision?
            _eI = _eI + TS1*_e;     // integral of e(t)
//        }
    }
    u = _Kp4th*_e + _Kd4th*(_e-e_old)/TS1 + _Ki4th*_eI;   // PID output u(t)
 
#if CONTROL_MODE==1||CONTROL_MODE==2   // frequency response, or Step response
    wt = _freq_u *2.0*PI*_time;
    if(wt>2*PI)    wt -= 2*PI*(float)((int)(wt/2.0*PI));
    u = sin(wt ) * (UMAX-UMIN)/2.0 + (UMAX+UMIN)/2.0;
#endif
#if CONTROL_MODE==2   // Step response
    if( u>=0 )  u = IMAX/2.;
    else        u = IMIN/2.;
#endif
#if CONTROL_MODE==3                 // u=rand() to identify motor transfer function G(s) from V to angle
    if(count2==(int)(TS3/TS0)){
        u = ((float)rand()/RAND_MAX*2.0-1.0) * (UMAX-1.5)/2.0 + (UMAX+1.5)/2.0;
    }else{
        u = _iref;
    }
#endif
#if CONTROL_MODE==4                 // FFT identification, u=repetive signal
    if(count2==(int)(TS3/TS1)){
        u = data[count3][4];
    }else{
        u = _iref;
    }
#endif
    // u is saturated? for anti-windup
    if( u>IMAX ){
        _eI -= (u-IMAX)/_Ki4th;    if(_eI<0){   _eI=0;}
        u = IMAX;
//        _f_imax = 1;
    } else if( u<IMIN ){
        _eI -= (u-IMIN)/_Ki4th;    if(_eI>0){   _eI=0;}
        u = IMIN;
//        _f_imax = 1;
    }else{
        _f_imax = 0;
    }
    //-------- update data
    _th = y;
    _iref = u;
//debug[0] =_iref;
}
void i_controller() {    // if ticker. current controller & velocity controller
    void    u2Hbridge(float);    // input u to H bridge (full bridge) driver
#ifdef USE_CURRENT_CONTROL
    float   e_old;
    float   y, u;

//    _iref=_r*180/PI;  // step response from v to i, useful to tune PID gains.
    debug_p17 = 1;  // for debug: processing time check
//    if(debug_p17 == 1)  debug_p17=0;else    debug_p17=1;  // for debug: sampling time check

    _count+=1;
    // current PID controller
  #ifdef SIMULATION
    y = v_shunt_r/R_SHUNT;      // get i [A] from shunt resistance
  #else
    y = _iref;
  #endif
    if(_f_u_plus==0){   y=-y;}

    e_old = _ei;     // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
    _ei = _iref - y;    // error e(t)
    if( _f_umax==0 ){
        _eiI = _eiI + TS0*_ei;     // integral of e(t)
    }
    u = _Kp4i*_e + _Kd4i*(_ei-e_old)/TS0 + _Ki4i*_eiI;   // PID output u(t)

     // u is saturated? for anti-windup
    if( u>UMAX ){
        _eiI -= (u-UMAX)/_Ki4i;    if(_eiI<0){   _eiI=0;}
        u = UMAX;
//        _f_umax = 1;
    } else if( u<UMIN ){
        _eiI -= (u-UMIN)/_Ki4i;    if(_eiI>0){   _eiI=0;}
        u = UMIN;
//        _f_umax = 1;
    }else{
        _f_umax = 0;
    }
    //-------- update data
    _i = y;
    _u = u;
#else
    _u = _iref/IMAX*UMAX;     // without current control.
#endif

    u2Hbridge(_u);  // input u to TA7291 driver

    //-------- update data
    _time += TS0;    // time
//debug[0]=v_shunt_r; if(_f_u_plus==0){   debug[0]=-debug[0];}
    //-------- update data

    debug_p17 = 0;  // for debug: processing time check
}

void init_controller(){ // initialize controller parameters and signals
    u2Hbridge(0);           // initialize H bridge to stop mode
    _count=0;
    _time = 0;  // time
    _eI = _eiI = 0;    // reset integrater
    encoder.reset();    // set encoder counter zero
    _th = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
#ifdef  USE_PWM
    FIN.period( 1.0 / PWM_FREQ );   // PWM period [s]. Common to all PWM
#endif
}

void data2mbedUSB(){    // store data to save to mbedUSB after experiment is over
    if( _count_data<1000 ){
        data[_count_data][0]=_r; data[_count_data][1]=debug[0];
        data[_count_data][2]=_th; data[_count_data][3]=_time; data[_count_data][4]=_u;
        _count_data++;
    }
//BUG    for(j=0;j<19;j++){  fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]);
}
void display2PC(){  // display to tera term on PC
//    pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [Hz]\t%d\r\n", _time, il.vdq_ref[0], (int)(vl.w_lpf/(2*PI)+0.5), (int)(vl.w_ref/(2*PI)+0.5));  // print to tera term
    pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [deg]\t%8.2f\r\n", _time, _u, (int)(_th/(2*PI)*360.0), _r);//debug[0]*3.3/R_SHUNT);  // print to tera term
}
void timerTS2(void const *argument) {   // make sampling time TS2 timer (priority 2: precision 4ms)
    int ms;
    unsigned long c;
    while (true) {
        c = _count;
        //dummy();  // dummy() is called every TS2[s].
        if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
        Thread::wait(ms);
    }
}
void timerTS3(void const *argument) {   // make sampling time TS3 timer (priority 3: precision 4ms)
    int ms;
    unsigned long c;
    while (true) {
        c = _count;
        data2mbedUSB();  // dummy() is called every TS3[s].
        if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
        Thread::wait(ms);
    }
}
void timerTS4(void const *argument) {   // make sampling time TS4 timer (priority 4: precision 4ms)
    int ms;
    unsigned long c;
    while (true) {
        c = _count;
        display2PC();  // display to tera term on PC. display2PC() is called every TS4[s].
        if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){    ms=1;}
        Thread::wait(ms);
    }
}

void motor_control() {  // motor control ON for TMAX seconds.
    FILE *fp;   // save data to PC
    float   t=0;
#ifdef  GOOD_DATA
    int i;
    RtosTimer RtosTimerTS1(th_controller);  // RtosTimer priority is osPriorityAboveNormal, just one above main()

    count3=0;
    _count_data=0;
#endif
    init_controller(); // initialize controller parameters and signals
    _r = _r + _th;
//    if( _r>2*PI )    _r -= _r-2*PI;

    if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){   error( "" );} // save data to PC

    // start control (ON)
    TickerTimerTS0.attach(&i_controller, TS0 ); // Sampling period[s] of i_controller
    RtosTimerTS1.start((unsigned int)(TS1*1000.));   // Sampling period[ms] of th controller

    t = _time;
    while ( _time <= TMAX ) {
        // BUG!! Dangerous if TS3<0.1 because multi interrupt by fprintf is not prohibited! 1st aug of fprintf will be destroyed.
        //     fprintf returns before process completed.
//BUG   fprintf( fp, "%8.2f, %8.4f,\t%8.1f,\t%8.2f\r\n", disp[3], disp[1], disp[0], tmp);  // save data to PC (para, y, time, u)
#ifndef GOOD_DATA   // fprintf is dangerous because priority is higher than Ticker!
        fprintf( fp, "%f, %f, %f, %f, %f\r\n", _r, debug[0], _th, _time, _u);  // save data to PC (para, y, time, u)
#endif
        Thread::wait((unsigned int)((TS3-(_time-t))*1000.));  //[ms]
        t = _time;
    }
    // stop control (OFF)
    TickerTimerTS0.detach(); // timer interrupt stop
    RtosTimerTS1.stop();    // rtos timer stop
//    ThreadTimerTS3.terminate();   // if remove comment, mbed hangs up! why?
//    ThreadTimerTS4.terminate();   // if remove comment, mbed hangs up! why?

    init_controller(); // initialize controller parameters and signals
#ifdef  GOOD_DATA
    for(i=0;i<_count_data;i++){  fprintf( fp, "%f, %f, %f, %f, %f\r\n", data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]);}  // save data to PC (para, y, time, u)
#endif
    fclose( fp );               // release mbed USB drive
}

int main() {
//BUG(unstable!!)    Thread startTimerTS2(timerTS2,NULL,osPriorityNormal);
    Thread ThreadTimerTS3(timerTS3,NULL,osPriorityBelowNormal);
    Thread ThreadTimerTS4(timerTS4,NULL,osPriorityLow);
// Priority of Thread (RtosTimer is osPriorityAboveNormal)
//  osPriorityIdle          = -3,          ///< priority: idle (lowest)--> then, mbed ERROR!!
//  osPriorityLow           = -2,          ///< priority: low
//  osPriorityBelowNormal   = -1,          ///< priority: below normal
//  osPriorityNormal        =  0,          ///< priority: normal (default)
//  osPriorityAboveNormal   = +1,          ///< priority: above normal
//  osPriorityHigh          = +2,          ///< priority: high 
//  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
//  osPriorityError         =  0x84        ///< system cannot determine priority or thread has illegal priority
#if CONTROL_MODE==0     // PID control
    char    f;
    float   val;
#endif
#if CONTROL_MODE==4                 // FFT identification, u=repetive signal
    int i, j;
    float   max_u;
#endif

    while(true){
#if CONTROL_MODE==4                 // FFT identification, u=repetive signal
        max_u = 0;
        for( i=0;i<1000;i++ ){  // u=data[i][4]: memory for FFT identification input signal.
            data[i][4] = sin(_freq_u*2*PI * i*TS1);   // _u_freq = 10/2 * i [Hz]
            if( data[i][4]>max_u ){   max_u=data[i][4];}
        }
        for( j=1;j<50;j++ ){
            for( i=0;i<1000;i++ ){
                data[i][4] += sin((float)(j+1)*_freq_u*2*PI * i*TS1);
                if( data[i][4]>max_u ){   max_u=data[i][4];}
            }
        }
        for( i=0;i<1000;i++ ){
//            data[i][4] *= UMAX/max_u;
            data[i][4] = (data[i][4]/max_u+3)/4*UMAX;
        }
#endif
        pc.printf("Control start!!\r\n");
        motor_control();    // motor control ON for TMAX seconds.
        pc.printf("Control completed!!\r\n\r\n");
//        ThreadTimerTS3.terminate();
//        ThreadTimerTS4.terminate();

        // Change parameters using tera term
#if CONTROL_MODE>=1     // frequency response, or Step response
        pc.printf("Input u(t) Frequency[Hz]? (if 9, reset mbed)...");
        pc.scanf("%f",&_freq_u);
        pc.printf("%8.3f[Hz]\r\n", &_freq_u);  // print to tera term
        if(_freq_u==9){    mbed_reset();}
#else                   // PID control
//  #ifdef R_SIN
//        pc.printf("Reference signal r(t) Frequency[Hz]?...");
//        pc.scanf("%f",&_freq_u);
//        pc.printf("%8.3f[Hz]\r\n", _freq_u);  // print to tera term
//  #endif
        pc.printf("th-loop: Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp4th, _Ki4th, _Kd4th, _rmax*180./PI, _freq_u);
        pc.printf(" i-loop: Kp=%f, Ki=%f, Kd=%f\r\n",_Kp4i, _Ki4i, _Kd4i);
        pc.printf("Which number do you like to change?\r\n ... 0)no change, 1)Kp, 2)Ki, 3)Kd, 4)r(t) freq.[Hz], 5)r(t) amp.[deg].\r\n     6)iKp, 7)iKi, 8)iKd, 9)reset mbed ?");
        f=pc.getc()-48; //int = char-48
        pc.printf("\r\n    Value?... ");
        if(f>=1&&f<=8){ pc.scanf("%f",&val);}
        pc.printf("%8.3f\r\n", val);
        if(f==1){    _Kp4th = val;}
        if(f==2){    _Ki4th = val;}
        if(f==3){    _Kd4th = val;}
        if(f==4){    _freq_u = val;}
        if(f==5){    _rmax = val/180.*PI;}
        if(f==6){    _Kp4i = val;}
        if(f==7){    _Ki4i = val;}
        if(f==8){    _Kd4i = val;}
        if(f==9){    mbed_reset();}
        pc.printf("th-loop: Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp4th, _Ki4th, _Kd4th, _rmax*180./PI, _freq_u);
        pc.printf(" i-loop: Kp=%f, Ki=%f, Kd=%f\r\n",_Kp4i, _Ki4i, _Kd4i);
#endif
    }    
}