manabu kosaka / Mbed 2 deprecated DCmotor2

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

Dependencies:   QEI mbed-rtos mbed

Fork of DCmotor by manabu kosaka

main.cpp@12:459af534d1ee, 2013-01-04 (annotated)

Committer:
kosaka
Date:
Fri Jan 04 12:00:48 2013 +0000
Revision:
12:459af534d1ee
Parent:
11:9747752435d1
Child:
13:ba71733c11d7
DC motor control program using TA7291P type H bridge driver and rotary encoder with A, B phase.;

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 12:459af534d1ee 2 // ver. 121224 by Kosaka lab.
kosaka 0:fe068497f773 3 #include "mbed.h"
kosaka 0:fe068497f773 4 #include "rtos.h"
kosaka 0:fe068497f773 5
kosaka 12:459af534d1ee 6 #include "controller.h"
kosaka 12:459af534d1ee 7 #include "Hbridge.h"
kosaka 0:fe068497f773 8
kosaka 0:fe068497f773 9
kosaka 12:459af534d1ee 10 Serial pc2(USBTX, USBRX); // Display on tera term in PC
kosaka 12:459af534d1ee 11 LocalFileSystem local("mbedUSBdrive"); // save data to mbed USB disk drive in PC
kosaka 12:459af534d1ee 12 Ticker TickerTimerTS0; // Timer interrupt using TIMER3, TS<0.001 is OK. Priority is higher than rtosTimer.
kosaka 12:459af534d1ee 13 unsigned char fTimerTS2ON=0, fTimerTS3ON=0, fTimerTS4ON=0; // ON/OFF flag for timerTS2, 3, 4.
kosaka 12:459af534d1ee 14 //DigitalOut debug_p24(p24); // p17 for debug
kosaka 0:fe068497f773 15
kosaka 7:613febb8f028 16 extern "C" void mbed_reset();
kosaka 3:b6b9b8c7dce6 17
kosaka 12:459af534d1ee 18 void CallTimerTS2(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms)
kosaka 12:459af534d1ee 19 int ms;
kosaka 12:459af534d1ee 20 unsigned long c;
kosaka 12:459af534d1ee 21 while (true) {
kosaka 12:459af534d1ee 22 c = _count;
kosaka 12:459af534d1ee 23 if( fTimerTS2ON ){
kosaka 12:459af534d1ee 24 timerTS2(); // timerTS2() is called every TS2[s].
kosaka 3:b6b9b8c7dce6 25 }
kosaka 12:459af534d1ee 26 if( (ms=(int)(TS2*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
kosaka 12:459af534d1ee 27 Thread::wait(ms);
kosaka 3:b6b9b8c7dce6 28 }
kosaka 8:b8b31e9b60c2 29 }
kosaka 12:459af534d1ee 30 void CallTimerTS3(void const *argument) { // make sampling time TS3 timer (priority 3: precision 4ms)
kosaka 12:459af534d1ee 31 int ms;
kosaka 12:459af534d1ee 32 unsigned long c;
kosaka 12:459af534d1ee 33 while (true) {
kosaka 12:459af534d1ee 34 c = _count;
kosaka 12:459af534d1ee 35 if( fTimerTS3ON ){
kosaka 12:459af534d1ee 36 timerTS3(); // timerTS3() is called every TS3[s].
kosaka 2:e056793d6fc5 37 }
kosaka 12:459af534d1ee 38 if( (ms=(int)(TS3*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
kosaka 12:459af534d1ee 39 Thread::wait(ms);
kosaka 0:fe068497f773 40 }
kosaka 0:fe068497f773 41 }
kosaka 0:fe068497f773 42
kosaka 12:459af534d1ee 43 void CallTimerTS4(void const *argument) { // make sampling time TS4 timer (priority 4: precision 4ms)
kosaka 12:459af534d1ee 44 int ms;
kosaka 12:459af534d1ee 45 unsigned long c;
kosaka 12:459af534d1ee 46 while (true) {
kosaka 12:459af534d1ee 47 c = _count;
kosaka 12:459af534d1ee 48 if( fTimerTS4ON ){
kosaka 12:459af534d1ee 49 timerTS4(); // timerTS4() is called every TS4[s].
kosaka 7:613febb8f028 50 }
kosaka 12:459af534d1ee 51 if( (ms=(int)(TS4*1000-(_count-c)*TS0*1000))<=0 ){ ms=1;}
kosaka 12:459af534d1ee 52 Thread::wait(ms);
kosaka 12:459af534d1ee 53 }
kosaka 12:459af534d1ee 54 }
kosaka 0:fe068497f773 55
kosaka 12:459af534d1ee 56 //#define OLD
kosaka 12:459af534d1ee 57 int main(){
kosaka 12:459af534d1ee 58 unsigned short i;
kosaka 12:459af534d1ee 59 FILE *fp = fopen("/mbedUSBdrive/data.csv", "w"); // save data to PC
kosaka 12:459af534d1ee 60 RtosTimer RtosTimerTS1(timerTS1); // RtosTimer priority is osPriorityAboveNormal, just one above main()
kosaka 12:459af534d1ee 61 Thread ThreadTimerTS3(CallTimerTS3,NULL,osPriorityBelowNormal);
kosaka 12:459af534d1ee 62 Thread ThreadTimerTS4(CallTimerTS4,NULL,osPriorityLow);
kosaka 12:459af534d1ee 63 // Priority of Thread (RtosTimer is osPriorityAboveNormal)
kosaka 0:fe068497f773 64 // osPriorityIdle = -3, ///< priority: idle (lowest)--> then, mbed ERROR!!
kosaka 0:fe068497f773 65 // osPriorityLow = -2, ///< priority: low
kosaka 0:fe068497f773 66 // osPriorityBelowNormal = -1, ///< priority: below normal
kosaka 0:fe068497f773 67 // osPriorityNormal = 0, ///< priority: normal (default)
kosaka 0:fe068497f773 68 // osPriorityAboveNormal = +1, ///< priority: above normal
kosaka 0:fe068497f773 69 // osPriorityHigh = +2, ///< priority: high
kosaka 0:fe068497f773 70 // osPriorityRealtime = +3, ///< priority: realtime (highest)
kosaka 0:fe068497f773 71 // osPriorityError = 0x84 ///< system cannot determine priority or thread has illegal priority
kosaka 12:459af534d1ee 72
kosaka 12:459af534d1ee 73 // 指令速度
kosaka 12:459af534d1ee 74 float w_ref_req[2] = {2* 2*PI, 4* 2*PI}; // [rad/s]（第2要素は指令速度急変後の指令速度）
kosaka 12:459af534d1ee 75 float t; // current time
kosaka 12:459af534d1ee 76
kosaka 12:459af534d1ee 77 // IPMSMの機器定数等の設定, 制御器の初期化
kosaka 12:459af534d1ee 78 init_parameters();
kosaka 12:459af534d1ee 79
kosaka 12:459af534d1ee 80 // シミュレーション開始
kosaka 12:459af534d1ee 81 pc2.printf("Simulation start!!\r\n");
kosaka 12:459af534d1ee 82 #ifndef OLD
kosaka 12:459af534d1ee 83 // start control (ON)
kosaka 12:459af534d1ee 84 start_pwm();
kosaka 12:459af534d1ee 85 TickerTimerTS0.attach(&timerTS0, TS0 ); // Sampling period[s] of i_controller
kosaka 12:459af534d1ee 86 RtosTimerTS1.start((unsigned int)(TS1*1000.)); // Sampling period[ms] of th controller
kosaka 12:459af534d1ee 87 fTimerTS3ON = 1; // timerTS3 start
kosaka 12:459af534d1ee 88 fTimerTS4ON = 1; // timerTS3 start
kosaka 12:459af534d1ee 89 #endif
kosaka 12:459af534d1ee 90
kosaka 12:459af534d1ee 91 while( (t = _count*TS0) < TMAX ){
kosaka 12:459af534d1ee 92 // debug_p24 = 1;
kosaka 12:459af534d1ee 93
kosaka 12:459af534d1ee 94 // 速度急変
kosaka 12:459af534d1ee 95 if( 0.26<=t && t<2.3 ){
kosaka 12:459af534d1ee 96 vl.w_ref=w_ref_req[1]; // 目標速度をメインルーチンから速度制御メインループへ渡す。
kosaka 12:459af534d1ee 97 }else{
kosaka 12:459af534d1ee 98 vl.w_ref=w_ref_req[0];
kosaka 12:459af534d1ee 99 }
kosaka 12:459af534d1ee 100 #ifdef SIMULATION
kosaka 12:459af534d1ee 101 // 負荷トルク急変
kosaka 12:459af534d1ee 102 if( t<3.4 ){
kosaka 12:459af534d1ee 103 p.TL = 1;
kosaka 12:459af534d1ee 104 }else{
kosaka 12:459af534d1ee 105 p.TL = 2;
kosaka 12:459af534d1ee 106 }
kosaka 12:459af534d1ee 107 #endif
kosaka 12:459af534d1ee 108
kosaka 12:459af534d1ee 109 #ifdef OLD
kosaka 12:459af534d1ee 110 if( (++i2)>=(int)(TS1/TS0) ){ i2=0;
kosaka 12:459af534d1ee 111 timerTS1(&j); //velocity_loop(); // 速度制御メインループ(w_ref&beta_ref to idq_ref)
kosaka 12:459af534d1ee 112 }
kosaka 12:459af534d1ee 113 #endif
kosaka 12:459af534d1ee 114
kosaka 12:459af534d1ee 115 #ifdef OLD
kosaka 12:459af534d1ee 116 timerTS0();
kosaka 12:459af534d1ee 117 #endif
kosaka 12:459af534d1ee 118
kosaka 12:459af534d1ee 119 // debug_p24 = 0;
kosaka 12:459af534d1ee 120 Thread::wait(1);
kosaka 12:459af534d1ee 121 }
kosaka 12:459af534d1ee 122 // stop timers (OFF)
kosaka 12:459af534d1ee 123 stop_pwm();
kosaka 12:459af534d1ee 124 TickerTimerTS0.detach(); // timer interrupt stop
kosaka 12:459af534d1ee 125 RtosTimerTS1.stop(); // rtos timer stop
kosaka 12:459af534d1ee 126 fTimerTS3ON=0;//ThreadTimerTS3.terminate(); //
kosaka 12:459af534d1ee 127 fTimerTS4ON=0;//ThreadTimerTS4.terminate(); //
kosaka 12:459af534d1ee 128
kosaka 12:459af534d1ee 129 // save data to mbed USB drive
kosaka 12:459af534d1ee 130 for(i=0;i<_count_data;i++){
kosaka 12:459af534d1ee 131 fprintf( fp, "%f, %f, %f, %f, %f\r\n",
kosaka 12:459af534d1ee 132 data[i][0],data[i][1],data[i][2],data[i][3],data[i][4]); // save data to PC (para, y, time, u)
kosaka 12:459af534d1ee 133 }
kosaka 12:459af534d1ee 134 fclose( fp ); // release mbed USB drive
kosaka 12:459af534d1ee 135
kosaka 12:459af534d1ee 136 Thread::wait(100);
kosaka 12:459af534d1ee 137 pc2.printf("Control completed!!\r\n\r\n");
kosaka 0:fe068497f773 138 }