manabu kosaka / Mbed 2 deprecated DCmotor

DC motor control program using TA7291P type driver and rotary encoder with A, B phase.

Dependencies:   QEI mbed-rtos mbed

main.cpp@17:10be89ff33e8, 2012-12-08 (annotated)

Committer:
kosaka
Date:
Sat Dec 08 05:57:32 2012 +0000
Revision:
17:10be89ff33e8
Parent:
16:759d6f647c83 
debug;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kosaka 1:b91aeb5673f3 1 // DC motor control program using H-bridge driver (ex. TA7291P) and 360 resolution rotary encoder with A, B phase.
kosaka 17:10be89ff33e8 2 // ver. 121208c by Kosaka lab.
kosaka 0:fe068497f773 3 #include "mbed.h"
kosaka 0:fe068497f773 4 #include "rtos.h"
kosaka 0:fe068497f773 5 #include "QEI.h"
kosaka 0:fe068497f773 6 #define PI 3.14159265358979 // def. of PI
kosaka 0:fe068497f773 7 /*********** User setting for control parameters (begin) ***************/
kosaka 4:6ccbf4d3cb6d 8 //#define SIMULATION // Comment this line if not simulation
kosaka 3:b6b9b8c7dce6 9 #define USE_PWM // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0. Comment if use Vref=analog mode
kosaka 8:b8b31e9b60c2 10 #define PWM_FREQ 10000.0 //[Hz], pwm freq. available if USE_PWM is defined.
kosaka 15:744a81d5b7ac 11 //#define USE_CURRENT_CONTROL // Current control on. Comment if current control off.
kosaka 8:b8b31e9b60c2 12 #define CONTROL_MODE 0 // 0:PID control, 1:Frequency response, 2:Step response, 3. u=Rand to identify G(s), 4) FFT identification
kosaka 11:0984c90b820b 13 #define DEADZONE_PLUS 1. // deadzone of plus side
kosaka 11:0984c90b820b 14 #define DEADZONE_MINUS -1.5 // deadzone of minus side
kosaka 0:fe068497f773 15 #define GOOD_DATA // Comment this line if the length of data TMAX/TS2 > 1000
kosaka 6:16bee943a9fa 16 //#define R_SIN // Comment this line if r=step, not r = sin
kosaka 0:fe068497f773 17 float _freq_u = 0.3; // [Hz], freq. of Frequency response, or Step response
kosaka 15:744a81d5b7ac 18 float _rmax=360./180.*PI; // [rad], max. of reference signal
kosaka 8:b8b31e9b60c2 19 float _Kp4th=20; // P gain for PID from motor volt. to angle.
kosaka 8:b8b31e9b60c2 20 float _Ki4th=20; // I gain for PID from motor volt. to angle.
kosaka 8:b8b31e9b60c2 21 float _Kd4th=5; // D gain for PID from motor volt. to angle.
kosaka 8:b8b31e9b60c2 22 float _Kp4i=10.0; // P gain for PID from motor volt. to motor current.
kosaka 8:b8b31e9b60c2 23 float _Ki4i=10.0; // I gain for PID from motor volt. to motor current.
kosaka 8:b8b31e9b60c2 24 float _Kd4i=0.0; // D gain for PID from motor volt. to motor current.
kosaka 15:744a81d5b7ac 25 #define TS0 0.001//08//8 // [s], sampling time (priority highest: Ticker IRQ) of motor current i control PID using timer interrupt
kosaka 15:744a81d5b7ac 26 #define TS1 0.002//2//0.01 // [s], sampling time (priority high: RtosTimer) of motor angle th PID using rtos-timer
kosaka 15:744a81d5b7ac 27 #define TS2 0.05 // [s], sampling time (priority =main(): precision 4ms)
kosaka 15:744a81d5b7ac 28 #define TS3 0.02 // [s], sampling time (priority low: precision 4ms) to save data to PC using thread. But, max data length is 1000.
kosaka 15:744a81d5b7ac 29 #define TS4 0.2 // [s], sampling time (priority lowest: precision 4ms) to display data to PC tera term
kosaka 0:fe068497f773 30 #define TMAX 10 // [s], experiment starts from 0[s] to TMAX[s]
kosaka 0:fe068497f773 31 #define UMAX 3.3 // [V], max of control input u
kosaka 0:fe068497f773 32 #define UMIN -3.3 // [V], max of control input u
kosaka 8:b8b31e9b60c2 33 #define IMAX 0.5 // [A], max of motor current i
kosaka 8:b8b31e9b60c2 34 #define IMIN -0.5 // [A], max of motor current i
kosaka 3:b6b9b8c7dce6 35 #define DEADTIME 0.0001 // [s], deadtime to be set between plus volt. to/from minus
kosaka 3:b6b9b8c7dce6 36 // H bridge port setting
kosaka 3:b6b9b8c7dce6 37 #define FIN_PORT p21 // FIN (IN1) port of mbed
kosaka 3:b6b9b8c7dce6 38 #define RIN_PORT p22 // RIN (IN2) port of mbed
kosaka 3:b6b9b8c7dce6 39 #define VREF_PORT p18 // Vref port of mbed (available if USE_PWM is not defined)
kosaka 0:fe068497f773 40 DigitalOut debug_p17(p17); // p17 for debug
kosaka 15:744a81d5b7ac 41 DigitalOut debug_p23(p23); // p17 for debug
kosaka 15:744a81d5b7ac 42 DigitalOut debug_p24(p24); // p17 for debug
kosaka 8:b8b31e9b60c2 43 AnalogIn v_shunt_r(p19); // *3.3 [V], Volt of shunt R_SHUNT[Ohm]. The motor current i = v_shunt_r/R_SHUNT [A]
kosaka 8:b8b31e9b60c2 44 #define R_SHUNT 1.25 // [Ohm], shunt resistanse
kosaka 8:b8b31e9b60c2 45 //AnalogIn VCC(p19); // *3.3 [V], Volt of VCC for motor
kosaka 8:b8b31e9b60c2 46 //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.
kosaka 0:fe068497f773 47
kosaka 5:2e53814aae4c 48 #define N_ENC (24*4) // "*4": QEI::X4_ENCODING. Number of pulses in one revolution(=360 deg) of rotary encoder.
kosaka 0:fe068497f773 49 QEI encoder (p29, p30, NC, N_ENC, QEI::X4_ENCODING);
kosaka 0:fe068497f773 50 // QEI(PinName channelA, mbed pin for channel A input.
kosaka 0:fe068497f773 51 // PinName channelB, mbed pin for channel B input.
kosaka 0:fe068497f773 52 // PinName index, mbed pin for channel Z input. (index channel input Z phase th=0), (pass NC if not needed).
kosaka 0:fe068497f773 53 // int pulsesPerRev, Number of pulses in one revolution(=360 deg).
kosaka 0:fe068497f773 54 // Encoding encoding = X2_ENCODING, X2 is default. X2 uses interrupts on the rising and falling edges of only channel A where as
kosaka 0:fe068497f773 55 // X4 uses them on both channels.
kosaka 0:fe068497f773 56 // )
kosaka 0:fe068497f773 57 // void reset (void)
kosaka 0:fe068497f773 58 // Reset the encoder.
kosaka 0:fe068497f773 59 // int getCurrentState (void)
kosaka 0:fe068497f773 60 // Read the state of the encoder.
kosaka 0:fe068497f773 61 // int getPulses (void)
kosaka 0:fe068497f773 62 // Read the number of pulses recorded by the encoder.
kosaka 0:fe068497f773 63 // int getRevolutions (void)
kosaka 0:fe068497f773 64 // Read the number of revolutions recorded by the encoder on the index channel.
kosaka 0:fe068497f773 65 /*********** User setting for control parameters (end) ***************/
kosaka 0:fe068497f773 66
kosaka 0:fe068497f773 67
kosaka 0:fe068497f773 68 Serial pc(USBTX, USBRX); // Display on tera term in PC
kosaka 0:fe068497f773 69 LocalFileSystem local("local"); // save data to mbed USB disk drive in PC
kosaka 0:fe068497f773 70 //Semaphore semaphore1(1); // wait and release to protect memories and so on
kosaka 0:fe068497f773 71 //Mutex stdio_mutex; // wait and release to protect memories and so on
kosaka 15:744a81d5b7ac 72 Ticker TickerTimerTS0; // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer.
kosaka 0:fe068497f773 73
kosaka 3:b6b9b8c7dce6 74 #ifdef USE_PWM // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0.
kosaka 3:b6b9b8c7dce6 75 PwmOut FIN(FIN_PORT); // PWM for FIN, RIN=0 when forward rotation. H bridge driver PWM mode
kosaka 3:b6b9b8c7dce6 76 PwmOut RIN(RIN_PORT); // PWM for RIN, FIN=0 when reverse rotation. H bridge driver PWM mode
kosaka 3:b6b9b8c7dce6 77 #else // H bridge Vref=analog mode
kosaka 3:b6b9b8c7dce6 78 DigitalOut FIN(FIN_PORT);// FIN for DC motor H bridge driver. FIN=1, RIN=0 then forward rotation
kosaka 3:b6b9b8c7dce6 79 DigitalOut RIN(RIN_PORT);// RIN for DC motor H bridge driver. FIN=0, RIN=1 then reverse rotation
kosaka 3:b6b9b8c7dce6 80 #endif
kosaka 4:6ccbf4d3cb6d 81 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]
kosaka 4:6ccbf4d3cb6d 82
kosaka 0:fe068497f773 83 unsigned long _count; // sampling number
kosaka 0:fe068497f773 84 float _time; // time[s]
kosaka 1:b91aeb5673f3 85 float _r; // reference signal
kosaka 8:b8b31e9b60c2 86 float _th=0; // [rad], motor angle, control output of angle controller
kosaka 8:b8b31e9b60c2 87 float _i=0; // [A], motor current, control output of current controller
kosaka 0:fe068497f773 88 float _e=0; // e=r-y for PID controller
kosaka 0:fe068497f773 89 float _eI=0; // integral of e for PID controller
kosaka 8:b8b31e9b60c2 90 float _iref; // reference current iref [A], output of angle th_contorller
kosaka 8:b8b31e9b60c2 91 float _u; // control input[V], motor input volt.
kosaka 8:b8b31e9b60c2 92 float _ei=0; // e=r-y for current PID controller
kosaka 8:b8b31e9b60c2 93 float _eiI=0; // integral of e for current PID controller
kosaka 0:fe068497f773 94 unsigned char _f_u_plus=1;// sign(u)
kosaka 0:fe068497f773 95 unsigned char _f_umax=0;// flag showing u is max or not
kosaka 8:b8b31e9b60c2 96 unsigned char _f_imax=0;// flag showing i is max or not
kosaka 15:744a81d5b7ac 97 float debug[20]; // for debug
kosaka 0:fe068497f773 98 float disp[10]; // for printf to avoid interrupted by quicker process
kosaka 0:fe068497f773 99 #ifdef GOOD_DATA
kosaka 0:fe068497f773 100 float data[1000][5]; // memory to save data offline instead of "online fprintf".
kosaka 0:fe068497f773 101 unsigned int count3; //
kosaka 15:744a81d5b7ac 102 unsigned int count2=(int)(TS3/TS0); //
kosaka 15:744a81d5b7ac 103 unsigned int _count_data=0; // data2mbedUSB()
kosaka 0:fe068497f773 104 #endif
kosaka 0:fe068497f773 105
kosaka 7:613febb8f028 106 extern "C" void mbed_reset();
kosaka 3:b6b9b8c7dce6 107
kosaka 3:b6b9b8c7dce6 108 void u2Hbridge(float u){// input u to H bridge driver
kosaka 3:b6b9b8c7dce6 109 float duty;
kosaka 3:b6b9b8c7dce6 110 unsigned int f_deadtime, f_in, r_in;
kosaka 0:fe068497f773 111
kosaka 17:10be89ff33e8 112 // u=-u; // Set PWM Minus logic for smooth shunt current
kosaka 0:fe068497f773 113 if( u > 0 ){ // forward: rotate to plus
kosaka 11:0984c90b820b 114 u += DEADZONE_PLUS; // deadzone compensation
kosaka 3:b6b9b8c7dce6 115 duty = u/3.3; // Vref
kosaka 8:b8b31e9b60c2 116 if(_f_u_plus==0){ // if plus to/from minus, set FIN=RIN=0/1 for deadtime 100[us].
kosaka 3:b6b9b8c7dce6 117 f_deadtime = 1; // deadtime is required
kosaka 3:b6b9b8c7dce6 118 _f_u_plus=1;
kosaka 3:b6b9b8c7dce6 119 }else{
kosaka 3:b6b9b8c7dce6 120 f_deadtime = 0; // deadtime is required
kosaka 3:b6b9b8c7dce6 121 }
kosaka 3:b6b9b8c7dce6 122 f_in=1; r_in=0; // set forward direction
kosaka 0:fe068497f773 123 }else if( u < 0 ){ // reverse: rotate to minus
kosaka 11:0984c90b820b 124 u += DEADZONE_MINUS;// deadzone compensation
kosaka 3:b6b9b8c7dce6 125 duty = -u/3.3;
kosaka 8:b8b31e9b60c2 126 if(_f_u_plus==1){ // if plus to/from minus, set FIN=RIN=0/1 for deadtime 100[us].
kosaka 3:b6b9b8c7dce6 127 f_deadtime = 1; // deadtime is required
kosaka 3:b6b9b8c7dce6 128 _f_u_plus=0;
kosaka 3:b6b9b8c7dce6 129 }else{
kosaka 3:b6b9b8c7dce6 130 f_deadtime = 0; // deadtime is required
kosaka 3:b6b9b8c7dce6 131 }
kosaka 3:b6b9b8c7dce6 132 f_in=0; r_in=1; // set reverse direction
kosaka 0:fe068497f773 133 }else{// if( u == 0 ){ // stop mode
kosaka 3:b6b9b8c7dce6 134 duty = 0;
kosaka 3:b6b9b8c7dce6 135 f_deadtime = 0; // deadtime is required
kosaka 3:b6b9b8c7dce6 136 f_in=0; r_in=0; // set FIN & RIN
kosaka 0:fe068497f773 137 }
kosaka 3:b6b9b8c7dce6 138
kosaka 3:b6b9b8c7dce6 139 if( f_deadtime==1 ){// making deadtime
kosaka 3:b6b9b8c7dce6 140 FIN=0; RIN=0; // set upper&lower arm zero
kosaka 3:b6b9b8c7dce6 141 wait(DEADTIME);
kosaka 3:b6b9b8c7dce6 142 }
kosaka 3:b6b9b8c7dce6 143 #ifdef USE_PWM // H bridge PWM mode: Vref=Vcc, FIN,2 = PWM or 0
kosaka 3:b6b9b8c7dce6 144 FIN = duty*(float)f_in; RIN = duty*(float)r_in; // setting pwm FIN & RIN
kosaka 17:10be89ff33e8 145 // FIN = (1-duty*(float)f_in); RIN = (1-duty*(float)r_in); // setting pwm FIN & RIN. Minus logic for smooth shunt current
kosaka 4:6ccbf4d3cb6d 146 analog_out = 1; // setting Vref=UMAX, but Vref=Vcc is better.
kosaka 3:b6b9b8c7dce6 147 #else // Analog mode: Vref=analog, FIN, RIN = 1 or 0)
kosaka 3:b6b9b8c7dce6 148 FIN = f_in; RIN = r_in; // setting FIN & RIN
kosaka 3:b6b9b8c7dce6 149 analog_out = duty; // setting Vref : PID write DA, range is 0-1. Output voltage 0-3.3v
kosaka 3:b6b9b8c7dce6 150 #endif
kosaka 0:fe068497f773 151 }
kosaka 0:fe068497f773 152
kosaka 8:b8b31e9b60c2 153 void th_controller(void const *argument) { // if rtos. current controller & velocity controller
kosaka 0:fe068497f773 154 float e_old, wt;
kosaka 8:b8b31e9b60c2 155 float y, u;
kosaka 0:fe068497f773 156
kosaka 11:0984c90b820b 157 // y_old = _th; // y_old=y(t-TS) is older than y by 1 sampling time TS[s]. update data
kosaka 0:fe068497f773 158 #ifdef SIMULATION
kosaka 15:744a81d5b7ac 159 if( (u=_iref)>IMAX ){ u-=IMAX;}else if(u<IMIN){ u+=IMIN;}
kosaka 15:744a81d5b7ac 160 y = _th + TS1/10*(20*u-_th); //=(1-TS/0.1)*_y + 0.2*TS/0.1*_iref; // G = 20/(10s+1)
kosaka 15:744a81d5b7ac 161 debug[0] =_iref;
kosaka 0:fe068497f773 162 #else
kosaka 0:fe068497f773 163 // semaphore1.wait(); //
kosaka 0:fe068497f773 164 y = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
kosaka 0:fe068497f773 165 // semaphore1.release(); //
kosaka 0:fe068497f773 166 #endif
kosaka 8:b8b31e9b60c2 167 #define RMIN 0
kosaka 0:fe068497f773 168 wt = _freq_u *2.0*PI*_time;
kosaka 11:0984c90b820b 169 if(wt>2.0*PI){ wt -= 2.0*PI*(float)((int)(wt/(2.0*PI)));}
kosaka 1:b91aeb5673f3 170 _r = sin(wt ) * (_rmax-RMIN)/2.0 + (_rmax+RMIN)/2.0;
kosaka 1:b91aeb5673f3 171 #ifndef R_SIN
kosaka 1:b91aeb5673f3 172 if( _r>=(_rmax+RMIN)/2.0 ) _r = _rmax;
kosaka 1:b91aeb5673f3 173 else _r = 0;
kosaka 0:fe068497f773 174 #endif
kosaka 11:0984c90b820b 175 e_old = _e; // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
kosaka 0:fe068497f773 176 _e = _r - y; // error e(t)
kosaka 15:744a81d5b7ac 177 //debug[0]=_e;
kosaka 11:0984c90b820b 178 if( _e<((360.0/N_ENC)/180*PI) && _e>-((360.0/N_ENC)/180*PI) ){ // e is inside minimum precision?
kosaka 11:0984c90b820b 179 _e = 0;
kosaka 11:0984c90b820b 180 }
kosaka 8:b8b31e9b60c2 181 if( _f_imax==0 ){ // u is saturated?
kosaka 11:0984c90b820b 182 // if( _e>((360.0/N_ENC)/180*PI) || _e<-((360.0/N_ENC)/180*PI) ){ // e is inside minimum precision?
kosaka 14:1196c2d455ae 183 _eI = _eI + TS1*_e; // integral of e(t)
kosaka 11:0984c90b820b 184 // }
kosaka 0:fe068497f773 185 }
kosaka 14:1196c2d455ae 186 u = _Kp4th*_e + _Kd4th*(_e-e_old)/TS1 + _Ki4th*_eI; // PID output u(t)
kosaka 0:fe068497f773 187
kosaka 6:16bee943a9fa 188 #if CONTROL_MODE==1||CONTROL_MODE==2 // frequency response, or Step response
kosaka 0:fe068497f773 189 wt = _freq_u *2.0*PI*_time;
kosaka 0:fe068497f773 190 if(wt>2*PI) wt -= 2*PI*(float)((int)(wt/2.0*PI));
kosaka 0:fe068497f773 191 u = sin(wt ) * (UMAX-UMIN)/2.0 + (UMAX+UMIN)/2.0;
kosaka 0:fe068497f773 192 #endif
kosaka 0:fe068497f773 193 #if CONTROL_MODE==2 // Step response
kosaka 15:744a81d5b7ac 194 if( u>=0 ) u = IMAX/2.;
kosaka 15:744a81d5b7ac 195 else u = IMIN/2.;
kosaka 0:fe068497f773 196 #endif
kosaka 6:16bee943a9fa 197 #if CONTROL_MODE==3 // u=rand() to identify motor transfer function G(s) from V to angle
kosaka 15:744a81d5b7ac 198 if(count2==(int)(TS3/TS0)){
kosaka 6:16bee943a9fa 199 u = ((float)rand()/RAND_MAX*2.0-1.0) * (UMAX-1.5)/2.0 + (UMAX+1.5)/2.0;
kosaka 6:16bee943a9fa 200 }else{
kosaka 8:b8b31e9b60c2 201 u = _iref;
kosaka 6:16bee943a9fa 202 }
kosaka 6:16bee943a9fa 203 #endif
kosaka 7:613febb8f028 204 #if CONTROL_MODE==4 // FFT identification, u=repetive signal
kosaka 15:744a81d5b7ac 205 if(count2==(int)(TS3/TS1)){
kosaka 7:613febb8f028 206 u = data[count3][4];
kosaka 7:613febb8f028 207 }else{
kosaka 8:b8b31e9b60c2 208 u = _iref;
kosaka 7:613febb8f028 209 }
kosaka 7:613febb8f028 210 #endif
kosaka 8:b8b31e9b60c2 211 // u is saturated? for anti-windup
kosaka 8:b8b31e9b60c2 212 if( u>IMAX ){
kosaka 8:b8b31e9b60c2 213 _eI -= (u-IMAX)/_Ki4th; if(_eI<0){ _eI=0;}
kosaka 8:b8b31e9b60c2 214 u = IMAX;
kosaka 8:b8b31e9b60c2 215 // _f_imax = 1;
kosaka 8:b8b31e9b60c2 216 } else if( u<IMIN ){
kosaka 8:b8b31e9b60c2 217 _eI -= (u-IMIN)/_Ki4th; if(_eI>0){ _eI=0;}
kosaka 8:b8b31e9b60c2 218 u = IMIN;
kosaka 8:b8b31e9b60c2 219 // _f_imax = 1;
kosaka 8:b8b31e9b60c2 220 }else{
kosaka 8:b8b31e9b60c2 221 _f_imax = 0;
kosaka 8:b8b31e9b60c2 222 }
kosaka 8:b8b31e9b60c2 223 //-------- update data
kosaka 8:b8b31e9b60c2 224 _th = y;
kosaka 8:b8b31e9b60c2 225 _iref = u;
kosaka 15:744a81d5b7ac 226 //debug[0] =_iref;
kosaka 8:b8b31e9b60c2 227 }
kosaka 8:b8b31e9b60c2 228 void i_controller() { // if ticker. current controller & velocity controller
kosaka 8:b8b31e9b60c2 229 void u2Hbridge(float); // input u to H bridge (full bridge) driver
kosaka 8:b8b31e9b60c2 230 #ifdef USE_CURRENT_CONTROL
kosaka 8:b8b31e9b60c2 231 float e_old;
kosaka 8:b8b31e9b60c2 232 float y, u;
kosaka 8:b8b31e9b60c2 233
kosaka 8:b8b31e9b60c2 234 // _iref=_r*180/PI; // step response from v to i, useful to tune PID gains.
kosaka 8:b8b31e9b60c2 235 debug_p17 = 1; // for debug: processing time check
kosaka 8:b8b31e9b60c2 236 // if(debug_p17 == 1) debug_p17=0;else debug_p17=1; // for debug: sampling time check
kosaka 8:b8b31e9b60c2 237
kosaka 8:b8b31e9b60c2 238 _count+=1;
kosaka 8:b8b31e9b60c2 239 // current PID controller
kosaka 15:744a81d5b7ac 240 #ifdef SIMULATION
kosaka 8:b8b31e9b60c2 241 y = v_shunt_r/R_SHUNT; // get i [A] from shunt resistance
kosaka 15:744a81d5b7ac 242 #else
kosaka 15:744a81d5b7ac 243 y = _iref;
kosaka 15:744a81d5b7ac 244 #endif
kosaka 8:b8b31e9b60c2 245 if(_f_u_plus==0){ y=-y;}
kosaka 8:b8b31e9b60c2 246
kosaka 11:0984c90b820b 247 e_old = _ei; // e_old=e(t-TS) is older than e by 1 sampling time TS[s]. update data
kosaka 8:b8b31e9b60c2 248 _ei = _iref - y; // error e(t)
kosaka 8:b8b31e9b60c2 249 if( _f_umax==0 ){
kosaka 14:1196c2d455ae 250 _eiI = _eiI + TS0*_ei; // integral of e(t)
kosaka 8:b8b31e9b60c2 251 }
kosaka 14:1196c2d455ae 252 u = _Kp4i*_e + _Kd4i*(_ei-e_old)/TS0 + _Ki4i*_eiI; // PID output u(t)
kosaka 8:b8b31e9b60c2 253
kosaka 8:b8b31e9b60c2 254 // u is saturated? for anti-windup
kosaka 8:b8b31e9b60c2 255 if( u>UMAX ){
kosaka 8:b8b31e9b60c2 256 _eiI -= (u-UMAX)/_Ki4i; if(_eiI<0){ _eiI=0;}
kosaka 8:b8b31e9b60c2 257 u = UMAX;
kosaka 8:b8b31e9b60c2 258 // _f_umax = 1;
kosaka 8:b8b31e9b60c2 259 } else if( u<UMIN ){
kosaka 8:b8b31e9b60c2 260 _eiI -= (u-UMIN)/_Ki4i; if(_eiI>0){ _eiI=0;}
kosaka 8:b8b31e9b60c2 261 u = UMIN;
kosaka 8:b8b31e9b60c2 262 // _f_umax = 1;
kosaka 8:b8b31e9b60c2 263 }else{
kosaka 8:b8b31e9b60c2 264 _f_umax = 0;
kosaka 8:b8b31e9b60c2 265 }
kosaka 8:b8b31e9b60c2 266 //-------- update data
kosaka 8:b8b31e9b60c2 267 _i = y;
kosaka 8:b8b31e9b60c2 268 _u = u;
kosaka 8:b8b31e9b60c2 269 #else
kosaka 15:744a81d5b7ac 270 _u = _iref/IMAX*UMAX; // without current control.
kosaka 8:b8b31e9b60c2 271 #endif
kosaka 8:b8b31e9b60c2 272
kosaka 8:b8b31e9b60c2 273 u2Hbridge(_u); // input u to TA7291 driver
kosaka 0:fe068497f773 274
kosaka 0:fe068497f773 275 //-------- update data
kosaka 14:1196c2d455ae 276 _time += TS0; // time
kosaka 15:744a81d5b7ac 277 //debug[0]=v_shunt_r; if(_f_u_plus==0){ debug[0]=-debug[0];}
kosaka 0:fe068497f773 278 //-------- update data
kosaka 0:fe068497f773 279
kosaka 0:fe068497f773 280 debug_p17 = 0; // for debug: processing time check
kosaka 0:fe068497f773 281 }
kosaka 0:fe068497f773 282
kosaka 13:4116d4b6c2a5 283 void init_controller(){ // initialize controller parameters and signals
kosaka 3:b6b9b8c7dce6 284 u2Hbridge(0); // initialize H bridge to stop mode
kosaka 0:fe068497f773 285 _count=0;
kosaka 0:fe068497f773 286 _time = 0; // time
kosaka 8:b8b31e9b60c2 287 _eI = _eiI = 0; // reset integrater
kosaka 1:b91aeb5673f3 288 encoder.reset(); // set encoder counter zero
kosaka 8:b8b31e9b60c2 289 _th = (float)encoder.getPulses()/(float)N_ENC*2.0*PI; // get angle [rad] from encoder
kosaka 13:4116d4b6c2a5 290 #ifdef USE_PWM
kosaka 13:4116d4b6c2a5 291 FIN.period( 1.0 / PWM_FREQ ); // PWM period [s]. Common to all PWM
kosaka 13:4116d4b6c2a5 292 #endif
kosaka 13:4116d4b6c2a5 293 }
kosaka 13:4116d4b6c2a5 294
kosaka 15:744a81d5b7ac 295 void data2mbedUSB(){ // store data to save to mbedUSB after experiment is over
kosaka 15:744a81d5b7ac 296 if( _count_data<1000 ){
kosaka 15:744a81d5b7ac 297 data[_count_data][0]=_r; data[_count_data][1]=debug[0];
kosaka 15:744a81d5b7ac 298 data[_count_data][2]=_th; data[_count_data][3]=_time; data[_count_data][4]=_u;
kosaka 15:744a81d5b7ac 299 _count_data++;
kosaka 0:fe068497f773 300 }
kosaka 15:744a81d5b7ac 301 //BUG for(j=0;j<19;j++){ fprintf( fp, "%f, ",debug[j]);} fprintf( fp, "%f\n",debug[19]);
kosaka 0:fe068497f773 302 }
kosaka 14:1196c2d455ae 303 void display2PC(){ // display to tera term on PC
kosaka 15:744a81d5b7ac 304 // 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
kosaka 14:1196c2d455ae 305 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
kosaka 14:1196c2d455ae 306 }
kosaka 15:744a81d5b7ac 307 void timerTS2(void const *argument) { // make sampling time TS2 timer (priority 2: precision 4ms)
kosaka 14:1196c2d455ae 308 int ms;
kosaka 14:1196c2d455ae 309 unsigned long c;
kosaka 0:fe068497f773 310 while (true) {
kosaka 14:1196c2d455ae 311 c = _count;
kosaka 15:744a81d5b7ac 312 //dummy(); // dummy() is called every TS2[s].
kosaka 15:744a81d5b7ac 313 if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
kosaka 15:744a81d5b7ac 314 Thread::wait(ms);
kosaka 15:744a81d5b7ac 315 }
kosaka 15:744a81d5b7ac 316 }
kosaka 15:744a81d5b7ac 317 void timerTS3(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms)
kosaka 15:744a81d5b7ac 318 int ms;
kosaka 15:744a81d5b7ac 319 unsigned long c;
kosaka 15:744a81d5b7ac 320 while (true) {
kosaka 15:744a81d5b7ac 321 c = _count;
kosaka 15:744a81d5b7ac 322 data2mbedUSB(); // dummy() is called every TS3[s].
kosaka 14:1196c2d455ae 323 if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
kosaka 14:1196c2d455ae 324 Thread::wait(ms);
kosaka 14:1196c2d455ae 325 }
kosaka 14:1196c2d455ae 326 }
kosaka 15:744a81d5b7ac 327 void timerTS4(void const *argument) { // make sampling time TS4 timer (priority 4: precision 4ms)
kosaka 14:1196c2d455ae 328 int ms;
kosaka 14:1196c2d455ae 329 unsigned long c;
kosaka 14:1196c2d455ae 330 while (true) {
kosaka 14:1196c2d455ae 331 c = _count;
kosaka 14:1196c2d455ae 332 display2PC(); // display to tera term on PC. display2PC() is called every TS4[s].
kosaka 14:1196c2d455ae 333 if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
kosaka 14:1196c2d455ae 334 Thread::wait(ms);
kosaka 0:fe068497f773 335 }
kosaka 0:fe068497f773 336 }
kosaka 0:fe068497f773 337
kosaka 15:744a81d5b7ac 338 void motor_control() { // motor control ON for TMAX seconds.
kosaka 15:744a81d5b7ac 339 FILE *fp; // save data to PC
kosaka 15:744a81d5b7ac 340 float t=0;
kosaka 15:744a81d5b7ac 341 #ifdef GOOD_DATA
kosaka 15:744a81d5b7ac 342 int i;
kosaka 15:744a81d5b7ac 343 RtosTimer RtosTimerTS1(th_controller); // RtosTimer priority is osPriorityAboveNormal, just one above main()
kosaka 15:744a81d5b7ac 344
kosaka 15:744a81d5b7ac 345 count3=0;
kosaka 15:744a81d5b7ac 346 _count_data=0;
kosaka 15:744a81d5b7ac 347 #endif
kosaka 15:744a81d5b7ac 348 init_controller(); // initialize controller parameters and signals
kosaka 15:744a81d5b7ac 349 _r = _r + _th;
kosaka 15:744a81d5b7ac 350 // if( _r>2*PI ) _r -= _r-2*PI;
kosaka 15:744a81d5b7ac 351
kosaka 15:744a81d5b7ac 352 if ( NULL == (fp = fopen( "/local/data.csv", "w" )) ){ error( "" );} // save data to PC
kosaka 15:744a81d5b7ac 353
kosaka 15:744a81d5b7ac 354 // start control (ON)
kosaka 15:744a81d5b7ac 355 TickerTimerTS0.attach(&i_controller, TS0 ); // Sampling period[s] of i_controller
kosaka 15:744a81d5b7ac 356 RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller
kosaka 15:744a81d5b7ac 357
kosaka 15:744a81d5b7ac 358 t = _time;
kosaka 15:744a81d5b7ac 359 while ( _time <= TMAX ) {
kosaka 15:744a81d5b7ac 360 // BUG!! Dangerous if TS3<0.1 because multi interrupt by fprintf is not prohibited! 1st aug of fprintf will be destroyed.
kosaka 15:744a81d5b7ac 361 // fprintf returns before process completed.
kosaka 15:744a81d5b7ac 362 //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)
kosaka 15:744a81d5b7ac 363 #ifndef GOOD_DATA // fprintf is dangerous because priority is higher than Ticker!
kosaka 15:744a81d5b7ac 364 fprintf( fp, "%f, %f, %f, %f, %f\r\n", _r, debug[0], _th, _time, _u); // save data to PC (para, y, time, u)
kosaka 15:744a81d5b7ac 365 #endif
kosaka 15:744a81d5b7ac 366 Thread::wait((unsigned int)((TS3-(_time-t))*1000.)); //[ms]
kosaka 15:744a81d5b7ac 367 t = _time;
kosaka 15:744a81d5b7ac 368 }
kosaka 15:744a81d5b7ac 369 // stop control (OFF)
kosaka 15:744a81d5b7ac 370 TickerTimerTS0.detach(); // timer interrupt stop
kosaka 15:744a81d5b7ac 371 RtosTimerTS1.stop(); // rtos timer stop
kosaka 15:744a81d5b7ac 372 // ThreadTimerTS3.terminate(); // if remove comment, mbed hangs up! why?
kosaka 15:744a81d5b7ac 373 // ThreadTimerTS4.terminate(); // if remove comment, mbed hangs up! why?
kosaka 15:744a81d5b7ac 374
kosaka 15:744a81d5b7ac 375 init_controller(); // initialize controller parameters and signals
kosaka 15:744a81d5b7ac 376 #ifdef GOOD_DATA
kosaka 15:744a81d5b7ac 377 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)
kosaka 15:744a81d5b7ac 378 #endif
kosaka 15:744a81d5b7ac 379 fclose( fp ); // release mbed USB drive
kosaka 15:744a81d5b7ac 380 }
kosaka 15:744a81d5b7ac 381
kosaka 15:744a81d5b7ac 382 int main() {
kosaka 16:759d6f647c83 383 //BUG(unstable!!) Thread startTimerTS2(timerTS2,NULL,osPriorityNormal);
kosaka 16:759d6f647c83 384 Thread ThreadTimerTS3(timerTS3,NULL,osPriorityBelowNormal);
kosaka 16:759d6f647c83 385 Thread ThreadTimerTS4(timerTS4,NULL,osPriorityLow);
kosaka 16:759d6f647c83 386 // Priority of Thread (RtosTimer is osPriorityAboveNormal)
kosaka 16:759d6f647c83 387 // osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!!
kosaka 16:759d6f647c83 388 // osPriorityLow = -2, ///< priority: low
kosaka 16:759d6f647c83 389 // osPriorityBelowNormal = -1, ///< priority: below normal
kosaka 16:759d6f647c83 390 // osPriorityNormal = 0, ///< priority: normal (default)
kosaka 16:759d6f647c83 391 // osPriorityAboveNormal = +1, ///< priority: above normal
kosaka 16:759d6f647c83 392 // osPriorityHigh = +2, ///< priority: high
kosaka 16:759d6f647c83 393 // osPriorityRealtime = +3, ///< priority: realtime (highest)
kosaka 16:759d6f647c83 394 // osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
kosaka 0:fe068497f773 395 #if CONTROL_MODE==0 // PID control
kosaka 0:fe068497f773 396 char f;
kosaka 0:fe068497f773 397 float val;
kosaka 0:fe068497f773 398 #endif
kosaka 7:613febb8f028 399 #if CONTROL_MODE==4 // FFT identification, u=repetive signal
kosaka 7:613febb8f028 400 int i, j;
kosaka 7:613febb8f028 401 float max_u;
kosaka 7:613febb8f028 402 #endif
kosaka 7:613febb8f028 403
kosaka 0:fe068497f773 404 while(true){
kosaka 7:613febb8f028 405 #if CONTROL_MODE==4 // FFT identification, u=repetive signal
kosaka 7:613febb8f028 406 max_u = 0;
kosaka 7:613febb8f028 407 for( i=0;i<1000;i++ ){ // u=data[i][4]: memory for FFT identification input signal.
kosaka 15:744a81d5b7ac 408 data[i][4] = sin(_freq_u*2*PI * i*TS1); // _u_freq = 10/2 * i [Hz]
kosaka 7:613febb8f028 409 if( data[i][4]>max_u ){ max_u=data[i][4];}
kosaka 7:613febb8f028 410 }
kosaka 7:613febb8f028 411 for( j=1;j<50;j++ ){
kosaka 7:613febb8f028 412 for( i=0;i<1000;i++ ){
kosaka 15:744a81d5b7ac 413 data[i][4] += sin((float)(j+1)*_freq_u*2*PI * i*TS1);
kosaka 7:613febb8f028 414 if( data[i][4]>max_u ){ max_u=data[i][4];}
kosaka 7:613febb8f028 415 }
kosaka 7:613febb8f028 416 }
kosaka 7:613febb8f028 417 for( i=0;i<1000;i++ ){
kosaka 7:613febb8f028 418 // data[i][4] *= UMAX/max_u;
kosaka 7:613febb8f028 419 data[i][4] = (data[i][4]/max_u+3)/4*UMAX;
kosaka 7:613febb8f028 420 }
kosaka 7:613febb8f028 421 #endif
kosaka 13:4116d4b6c2a5 422 pc.printf("Control start!!\r\n");
kosaka 15:744a81d5b7ac 423 motor_control(); // motor control ON for TMAX seconds.
kosaka 13:4116d4b6c2a5 424 pc.printf("Control completed!!\r\n\r\n");
kosaka 17:10be89ff33e8 425 // ThreadTimerTS3.terminate();
kosaka 17:10be89ff33e8 426 // ThreadTimerTS4.terminate();
kosaka 0:fe068497f773 427
kosaka 13:4116d4b6c2a5 428 // Change parameters using tera term
kosaka 0:fe068497f773 429 #if CONTROL_MODE>=1 // frequency response, or Step response
kosaka 7:613febb8f028 430 pc.printf("Input u(t) Frequency[Hz]? (if 9, reset mbed)...");
kosaka 0:fe068497f773 431 pc.scanf("%f",&_freq_u);
kosaka 16:759d6f647c83 432 pc.printf("%8.3f[Hz]\r\n", &_freq_u); // print to tera term
kosaka 7:613febb8f028 433 if(_freq_u==9){ mbed_reset();}
kosaka 0:fe068497f773 434 #else // PID control
kosaka 1:b91aeb5673f3 435 // #ifdef R_SIN
kosaka 1:b91aeb5673f3 436 // pc.printf("Reference signal r(t) Frequency[Hz]?...");
kosaka 1:b91aeb5673f3 437 // pc.scanf("%f",&_freq_u);
kosaka 1:b91aeb5673f3 438 // pc.printf("%8.3f[Hz]\r\n", _freq_u); // print to tera term
kosaka 1:b91aeb5673f3 439 // #endif
kosaka 8:b8b31e9b60c2 440 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);
kosaka 8:b8b31e9b60c2 441 pc.printf(" i-loop: Kp=%f, Ki=%f, Kd=%f\r\n",_Kp4i, _Ki4i, _Kd4i);
kosaka 8:b8b31e9b60c2 442 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 ?");
kosaka 0:fe068497f773 443 f=pc.getc()-48; //int = char-48
kosaka 0:fe068497f773 444 pc.printf("\r\n Value?... ");
kosaka 11:0984c90b820b 445 if(f>=1&&f<=8){ pc.scanf("%f",&val);}
kosaka 13:4116d4b6c2a5 446 pc.printf("%8.3f\r\n", val);
kosaka 8:b8b31e9b60c2 447 if(f==1){ _Kp4th = val;}
kosaka 8:b8b31e9b60c2 448 if(f==2){ _Ki4th = val;}
kosaka 8:b8b31e9b60c2 449 if(f==3){ _Kd4th = val;}
kosaka 1:b91aeb5673f3 450 if(f==4){ _freq_u = val;}
kosaka 1:b91aeb5673f3 451 if(f==5){ _rmax = val/180.*PI;}
kosaka 8:b8b31e9b60c2 452 if(f==6){ _Kp4i = val;}
kosaka 8:b8b31e9b60c2 453 if(f==7){ _Ki4i = val;}
kosaka 8:b8b31e9b60c2 454 if(f==8){ _Kd4i = val;}
kosaka 7:613febb8f028 455 if(f==9){ mbed_reset();}
kosaka 8:b8b31e9b60c2 456 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);
kosaka 8:b8b31e9b60c2 457 pc.printf(" i-loop: Kp=%f, Ki=%f, Kd=%f\r\n",_Kp4i, _Ki4i, _Kd4i);
kosaka 0:fe068497f773 458 #endif
kosaka 0:fe068497f773 459 }
kosaka 0:fe068497f773 460 }