File content as of revision 2:e056793d6fc5:

//  DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase.
//      ver. 121115a 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 CONTROL_MODE    0   // 0:PID control, 1:Frequency response, 2:Step response
#define GOOD_DATA           // Comment this line if the length of data TMAX/TS2 > 1000
//#define R_SIN               // Comment this line if not r = sin
float   _freq_u = 0.3;      // [Hz], freq. of Frequency response, or Step response
float   _rmax=100./180.*PI; // [rad], max. of reference signal
float   _Kp=70;             // P gain for PID ... Kp=1, Ki=0, Kd=0 is good.
float   _Ki=10;             // I gain for PID
float   _Kd=0.01;           // D gain for PID
#define TS      0.001       // [s], TS>0.001[s], sampling time[s] of PID controller
#define TS2     0.01        // [s], TS2>0.001[s], sampling time[s] of data save to PC. BUG!! Dangerous if TS2<0.1 because multi interrupt by fprintf is not prohibited! 1st aug of fprintf will be destroyed.
#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

AnalogOut   analog_out(p18);// Vref for DC motor driver TA7291P. DA converter for control input [0.0-1.0]% in the output range of 0.0 to 3.3[V]
DigitalOut  IN1(p19);       // IN1  for DC motor driver TA7291P
DigitalOut  IN2(p20);       // IN2  for DC motor driver TA7291P
DigitalOut  debug_p17(p17); // p17 for debug

#define N_ENC   (360*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 controller_ticker;     // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer.

unsigned long _count;   // sampling number
float   _time;          // time[s]
float   _r;             // reference signal
float   _y;             // control output
float   _e=0;           // e=r-y for PID controller
float   _eI=0;          // integral of e for PID controller
float   _u;             // control input[V]
unsigned char _f_u_plus=1;// sign(u)
unsigned char _f_umax=0;// flag showing u is max or not
float   debug[10];      // 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)(TS2/TS); // 
#endif

void u2TA7291P(float u){// input u to TA7291 driver
    float   abs_u;

    if( u > 0 ){        // forward: rotate to plus
        abs_u = u;          // Vref
        if(_f_u_plus==0){   _f_u_plus=1;  IN1=0;  IN2=0;  analog_out=0;  wait(0.0001);}  // if plus to/from minus, set IN1=IN2=0/1 for 100[us].
        IN1 = 1;
        IN2 = 0;
    }else if( u < 0 ){  // reverse: rotate to minus
        abs_u = -u;
        if(_f_u_plus==1){   _f_u_plus=0;  IN1=0;  IN2=0;  analog_out=0;  wait(0.0001);}  // if plus to/from minus, set IN1=IN2=0/1 for 100[us].
        IN1 = 0;
        IN2 = 1;
    }else{// if( u == 0 ){  // stop mode
        abs_u = 0;
        IN1 = 0;
        IN2 = 0;
    }
    analog_out = abs_u/3.3; // PID write DA, range is 0-1. Output voltage 0-3.3v
}

void controller(void const *argument) {    // if rtos. current controller & velocity controller
//void controller() {    // if ticker. current controller & velocity controller
    void    u2TA7291P(float);    // input u to TA7291 driver
    float   e_old, wt;
    float   y, u;     // to avoid time shift

    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;
//    y_old = _y;  // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data
#ifdef SIMULATION
    y = _y + TS/0.1*(0.02*_u*100-_y);   //=(1-TS/0.1)*_y + 0.02*TS/0.1*_u; // G = 0.02/(0.1s+1)
//debug[0]=_u;//plus
#else
//    semaphore1.wait();      //
    y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
//    semaphore1.release();   //
#endif
//#ifdef R_SIN
//  #define RMAX  (100./180.*PI)
  #define RMIN  0
    wt = _freq_u *2.0*PI*_time;
    if(wt>2*PI){    wt -= 2*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)
    if( _f_umax==0 ){
        _eI = _eI + TS*_e;     // integral of e(t)
    }
    
    u = _Kp*_e + _Kd*(_e-e_old)/TS + _Ki*_eI;   // PID output u(t)
//debug[0]=_e;//minus
//debug[0]=u;//minus
 
    // u is saturated? for anti-windup
    if( u>UMAX ){
        _eI -= (u-UMAX)/_Ki;    if(_eI<0){   _eI=0;}
        u = UMAX;
//        _f_umax = 1;
    } else if( u<UMIN ){
        _eI -= (u-UMIN)/_Ki;    if(_eI>0){   _eI=0;}
        u = UMIN;
//        _f_umax = 1;
    }else{
        _f_umax = 0;
    }
//#define CONTROL_MODE   2   // 0:PID control, 1:Frequency response, 2:Step response
#if CONTROL_MODE>=1   // 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 = UMAX;
    else        u = UMIN;
#endif
//debug[0]=u;//minus
    u2TA7291P(u);    // input u to TA7291 driver

    //-------- update data
    _time += TS;    // time
    _y = y;
    _u = u;
//debug[0]=_u;//minus
//debug[0]=_eI;
debug[0]=_r;
#ifdef  GOOD_DATA
    if(count2==(int)(TS2/TS)){
//        j=0; if(_count>=j&&_count<j+1000){i=_count-j;  data[i][0]=_r; data[i][1]=debug[0]; data[i][2]=_y; data[i][3]=_time; data[i][4]=_u;}
        if( count3<1000 ){
            data[count3][0]=_r; data[count3][1]=debug[0]; data[count3][2]=_y; data[count3][3]=_time; data[count3][4]=_u;
            count3++;
        }
        count2 = 0;
    }
    count2++;
#endif
    //-------- update data

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

void main1() {
    RtosTimer timer_controller(controller);
    FILE *fp;   // save data to PC
#ifdef  GOOD_DATA
    int i;

    count3=0;
#endif
    _count=0;
    _time = 0;  // time
    _e = _eI = 0;
    encoder.reset();    // set encoder counter zero
    _y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI;   // get angle [rad] from encoder
    _r = _r + _y;
//    if( _r>2*PI )    _r -= _r-2*PI;

    pc.printf("Control start!!\r\n");
    if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){   error( "" );} // save data to PC

//    controller_ticker.attach(&controller, TS ); // period [s]
    timer_controller.start((unsigned int)(TS*1000.));   // Sampling period[ms]

//    for ( i = 0; i < (unsigned int)(TMAX/TS2); i++ ) {
    while ( _time <= TMAX ) {
        // BUG!! Dangerous if TS2<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)
//OK?   fprintf( fp, "%f, %f, %f, %f, %f\r\n", _time, debug[0], debug[3], (_y/(2*PI)*360.0),_u);  // save data to PC (para, y, time, u)
#ifndef GOOD_DATA
        fprintf( fp, "%f, %f, %f, %f, %f\r\n", _r, debug[0], _y, _time, _u);  // save data to PC (para, y, time, u)
#endif
        Thread::wait((unsigned int)(TS2*1000.));  //[ms]
    }
    timer_controller.stop();    // rtos timer stop
    analog_out = 0;              // stop motor
#ifdef  GOOD_DATA
    for(i=0;i<1000;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
    pc.printf("Control completed!!\r\n\r\n");
}

void thread_print2PC(void const *argument) {
    while (true) {
        pc.printf("%8.1f[s]\t%8.5f[V]\t%4d [deg]\t%8.2f\r\n", _time, _u, (int)(_y/(2*PI)*360.0), debug[0]/(2*PI)*360.0);  // print to tera term
        Thread::wait(200);
    }
}

void main2(void const *argument) {
#if CONTROL_MODE==0     // PID control
    char    f;
    float   val;
#endif
    
    while(true){
        main1();

#if CONTROL_MODE>=1     // frequency response, or Step response
        pc.printf("Input u(t) Frequency[Hz]?...");
        pc.scanf("%f",&_freq_u);
        pc.printf("%8.3f[Hz]\r\n", _freq_u);  // print to tera term
#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("Which number do you like to change?\r\n ... 0)no change, 1)Kp, 2)Ki, 3)Kd, 4)freq.[Hz] of r(t), 5)amp.[deg] of r(t)?");
        f=pc.getc()-48; //int = char-48
        pc.printf("\r\n    Value?... ");
        if(f>=1&&f<=5){ pc.scanf("%f",&val);}
        pc.printf("%8.3f\r\n", val);  // print to tera term
        if(f==1){    _Kp = val;}
        if(f==2){    _Ki = val;}
        if(f==3){    _Kd = val;}
        if(f==4){    _freq_u = val;}
        if(f==5){    _rmax = val/180.*PI;}
        pc.printf("Kp=%f, Ki=%f, Kd=%f, r=%f[deg], %f Hz\r\n",_Kp, _Ki, _Kd, _rmax*180./PI, _freq_u);
#endif
    }    
}
int main() {
//    void main1();
    Thread save2PC(main2,NULL,osPriorityBelowNormal);
    Thread print2PC(thread_print2PC,NULL,osPriorityLow);

//    osStatus set_priority(osPriority osPriorityBelowNormal );
// Priority of Thread (RtosTimer has no priority?)
//  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
}