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?
User | Revision | Line number | New 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 | } |