akiyoshi oguro
/
Nucleo_sinwt_BLDC
AC_BLDC_MOTOR_Control
Fork of Nucleo_sinwt_BLDC_ by
main.cpp@0:3e8df216e6c8, 2017-09-22 (annotated)
- Committer:
- oguro
- Date:
- Fri Sep 22 00:42:54 2017 +0000
- Revision:
- 0:3e8df216e6c8
AC_BLDC_MOTOR_Control
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
oguro | 0:3e8df216e6c8 | 1 | #include "mbed.h" |
oguro | 0:3e8df216e6c8 | 2 | #include "rtos.h" |
oguro | 0:3e8df216e6c8 | 3 | #include <math.h> |
oguro | 0:3e8df216e6c8 | 4 | #define TS1 0.2 |
oguro | 0:3e8df216e6c8 | 5 | int q=0,START=15; //10 |
oguro | 0:3e8df216e6c8 | 6 | float ut1=0,ut2=0,usi=0; |
oguro | 0:3e8df216e6c8 | 7 | float vt1=0,vt2=0,vsi=0; |
oguro | 0:3e8df216e6c8 | 8 | float wt1=0,wt2=0,wsi=0; |
oguro | 0:3e8df216e6c8 | 9 | float ui=0,vi=0,wi=0; |
oguro | 0:3e8df216e6c8 | 10 | float su,sv,sw; |
oguro | 0:3e8df216e6c8 | 11 | float PI=3.141592; |
oguro | 0:3e8df216e6c8 | 12 | float t=2.26E-4; //2.26E-4 |
oguro | 0:3e8df216e6c8 | 13 | float Speed; |
oguro | 0:3e8df216e6c8 | 14 | PwmOut mypwmA(PA_8); //PWM_OUT 8 |
oguro | 0:3e8df216e6c8 | 15 | PwmOut mypwmB(PA_9); //9 |
oguro | 0:3e8df216e6c8 | 16 | PwmOut mypwmC(PA_10);//10 |
oguro | 0:3e8df216e6c8 | 17 | |
oguro | 0:3e8df216e6c8 | 18 | DigitalOut EN1(PC_10); |
oguro | 0:3e8df216e6c8 | 19 | DigitalOut EN2(PC_11); |
oguro | 0:3e8df216e6c8 | 20 | DigitalOut EN3(PC_12); |
oguro | 0:3e8df216e6c8 | 21 | |
oguro | 0:3e8df216e6c8 | 22 | InterruptIn HA(PA_15); |
oguro | 0:3e8df216e6c8 | 23 | InterruptIn HB(PB_3); |
oguro | 0:3e8df216e6c8 | 24 | InterruptIn HC(PB_10); |
oguro | 0:3e8df216e6c8 | 25 | |
oguro | 0:3e8df216e6c8 | 26 | AnalogIn V_adc(PC_2); //Potention |
oguro | 0:3e8df216e6c8 | 27 | //AnalogIn V_adc(PB_1); // gaibu Volume |
oguro | 0:3e8df216e6c8 | 28 | Timer uT; |
oguro | 0:3e8df216e6c8 | 29 | Timer vT; |
oguro | 0:3e8df216e6c8 | 30 | Timer wT; |
oguro | 0:3e8df216e6c8 | 31 | AnalogOut SWAVE(PA_4); |
oguro | 0:3e8df216e6c8 | 32 | |
oguro | 0:3e8df216e6c8 | 33 | Serial pc(USBTX,USBRX); |
oguro | 0:3e8df216e6c8 | 34 | |
oguro | 0:3e8df216e6c8 | 35 | DigitalOut myled(LED1); |
oguro | 0:3e8df216e6c8 | 36 | |
oguro | 0:3e8df216e6c8 | 37 | float Vr_adc=0.0f; |
oguro | 0:3e8df216e6c8 | 38 | |
oguro | 0:3e8df216e6c8 | 39 | void HAH(){ |
oguro | 0:3e8df216e6c8 | 40 | ut1=uT.read_us(); |
oguro | 0:3e8df216e6c8 | 41 | ui=0; |
oguro | 0:3e8df216e6c8 | 42 | } |
oguro | 0:3e8df216e6c8 | 43 | void HAL(){ |
oguro | 0:3e8df216e6c8 | 44 | ut2=uT.read_us(); |
oguro | 0:3e8df216e6c8 | 45 | uT.reset(); |
oguro | 0:3e8df216e6c8 | 46 | } |
oguro | 0:3e8df216e6c8 | 47 | void HBH(){ |
oguro | 0:3e8df216e6c8 | 48 | vt1=vT.read_us(); |
oguro | 0:3e8df216e6c8 | 49 | vi=0; |
oguro | 0:3e8df216e6c8 | 50 | } |
oguro | 0:3e8df216e6c8 | 51 | void HBL(){ |
oguro | 0:3e8df216e6c8 | 52 | vt2=vT.read_us(); |
oguro | 0:3e8df216e6c8 | 53 | vT.reset(); |
oguro | 0:3e8df216e6c8 | 54 | } |
oguro | 0:3e8df216e6c8 | 55 | void HCH(){ |
oguro | 0:3e8df216e6c8 | 56 | wt1=wT.read_us(); |
oguro | 0:3e8df216e6c8 | 57 | wi=0; |
oguro | 0:3e8df216e6c8 | 58 | } |
oguro | 0:3e8df216e6c8 | 59 | void HCL(){ |
oguro | 0:3e8df216e6c8 | 60 | wt2=wT.read_us(); |
oguro | 0:3e8df216e6c8 | 61 | wT.reset(); |
oguro | 0:3e8df216e6c8 | 62 | } |
oguro | 0:3e8df216e6c8 | 63 | |
oguro | 0:3e8df216e6c8 | 64 | void CPLT(){ |
oguro | 0:3e8df216e6c8 | 65 | pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc); |
oguro | 0:3e8df216e6c8 | 66 | } |
oguro | 0:3e8df216e6c8 | 67 | |
oguro | 0:3e8df216e6c8 | 68 | void timerTS1(void const*argument){ |
oguro | 0:3e8df216e6c8 | 69 | CPLT(); |
oguro | 0:3e8df216e6c8 | 70 | } |
oguro | 0:3e8df216e6c8 | 71 | |
oguro | 0:3e8df216e6c8 | 72 | |
oguro | 0:3e8df216e6c8 | 73 | int main() { |
oguro | 0:3e8df216e6c8 | 74 | |
oguro | 0:3e8df216e6c8 | 75 | pc.baud(128000); |
oguro | 0:3e8df216e6c8 | 76 | |
oguro | 0:3e8df216e6c8 | 77 | EN1=1; |
oguro | 0:3e8df216e6c8 | 78 | EN2=1; |
oguro | 0:3e8df216e6c8 | 79 | EN3=1; |
oguro | 0:3e8df216e6c8 | 80 | |
oguro | 0:3e8df216e6c8 | 81 | mypwmA.period_us(20); |
oguro | 0:3e8df216e6c8 | 82 | |
oguro | 0:3e8df216e6c8 | 83 | mypwmB.period_us(20); |
oguro | 0:3e8df216e6c8 | 84 | |
oguro | 0:3e8df216e6c8 | 85 | mypwmC.period_us(20); |
oguro | 0:3e8df216e6c8 | 86 | |
oguro | 0:3e8df216e6c8 | 87 | uT.start(); |
oguro | 0:3e8df216e6c8 | 88 | vT.start(); |
oguro | 0:3e8df216e6c8 | 89 | wT.start(); |
oguro | 0:3e8df216e6c8 | 90 | |
oguro | 0:3e8df216e6c8 | 91 | |
oguro | 0:3e8df216e6c8 | 92 | RtosTimer RtosTimerTS1(timerTS1); |
oguro | 0:3e8df216e6c8 | 93 | RtosTimerTS1.start((unsigned int)(TS1*3000)); |
oguro | 0:3e8df216e6c8 | 94 | Thread::wait(100); |
oguro | 0:3e8df216e6c8 | 95 | |
oguro | 0:3e8df216e6c8 | 96 | while(1) { |
oguro | 0:3e8df216e6c8 | 97 | |
oguro | 0:3e8df216e6c8 | 98 | Vr_adc=V_adc.read(); |
oguro | 0:3e8df216e6c8 | 99 | |
oguro | 0:3e8df216e6c8 | 100 | if((Vr_adc>0.15f)&&(q==0)){ |
oguro | 0:3e8df216e6c8 | 101 | |
oguro | 0:3e8df216e6c8 | 102 | while(q<50){ //30 |
oguro | 0:3e8df216e6c8 | 103 | |
oguro | 0:3e8df216e6c8 | 104 | mypwmA.write(0.5f); //0.5 |
oguro | 0:3e8df216e6c8 | 105 | mypwmB.write(0); |
oguro | 0:3e8df216e6c8 | 106 | mypwmC.write(0); |
oguro | 0:3e8df216e6c8 | 107 | wait_ms(START); |
oguro | 0:3e8df216e6c8 | 108 | |
oguro | 0:3e8df216e6c8 | 109 | mypwmA.write(0); |
oguro | 0:3e8df216e6c8 | 110 | mypwmB.write(0.5f); |
oguro | 0:3e8df216e6c8 | 111 | mypwmC.write(0); |
oguro | 0:3e8df216e6c8 | 112 | wait_ms(START); |
oguro | 0:3e8df216e6c8 | 113 | |
oguro | 0:3e8df216e6c8 | 114 | mypwmA.write(0); |
oguro | 0:3e8df216e6c8 | 115 | mypwmB.write(0); |
oguro | 0:3e8df216e6c8 | 116 | mypwmC.write(0.5f); |
oguro | 0:3e8df216e6c8 | 117 | wait_ms(START); |
oguro | 0:3e8df216e6c8 | 118 | q++; |
oguro | 0:3e8df216e6c8 | 119 | |
oguro | 0:3e8df216e6c8 | 120 | } |
oguro | 0:3e8df216e6c8 | 121 | // q=31; |
oguro | 0:3e8df216e6c8 | 122 | } |
oguro | 0:3e8df216e6c8 | 123 | |
oguro | 0:3e8df216e6c8 | 124 | HA.rise(&HAH); |
oguro | 0:3e8df216e6c8 | 125 | HC.fall(&HCL); |
oguro | 0:3e8df216e6c8 | 126 | HB.rise(&HBH); |
oguro | 0:3e8df216e6c8 | 127 | HA.fall(&HAL); |
oguro | 0:3e8df216e6c8 | 128 | HC.rise(&HCH); |
oguro | 0:3e8df216e6c8 | 129 | HB.fall(&HBL); |
oguro | 0:3e8df216e6c8 | 130 | |
oguro | 0:3e8df216e6c8 | 131 | if(Vr_adc < 0.05f){ |
oguro | 0:3e8df216e6c8 | 132 | q=0; |
oguro | 0:3e8df216e6c8 | 133 | |
oguro | 0:3e8df216e6c8 | 134 | } |
oguro | 0:3e8df216e6c8 | 135 | |
oguro | 0:3e8df216e6c8 | 136 | ui=ui+1; |
oguro | 0:3e8df216e6c8 | 137 | vi=vi+1; |
oguro | 0:3e8df216e6c8 | 138 | wi=wi+1; |
oguro | 0:3e8df216e6c8 | 139 | |
oguro | 0:3e8df216e6c8 | 140 | usi=ut2-ut1; |
oguro | 0:3e8df216e6c8 | 141 | vsi=vt2-vt1; |
oguro | 0:3e8df216e6c8 | 142 | wsi=wt2-wt1; |
oguro | 0:3e8df216e6c8 | 143 | |
oguro | 0:3e8df216e6c8 | 144 | if(q>=31){ |
oguro | 0:3e8df216e6c8 | 145 | |
oguro | 0:3e8df216e6c8 | 146 | su=sin(2*PI*((1/(2*usi*1E-6))*ui*t)); |
oguro | 0:3e8df216e6c8 | 147 | sv=sin(2*PI*((1/(2*vsi*1E-6))*vi*t)); |
oguro | 0:3e8df216e6c8 | 148 | sw=sin(2*PI*((1/(2*wsi*1E-6))*wi*t)); |
oguro | 0:3e8df216e6c8 | 149 | |
oguro | 0:3e8df216e6c8 | 150 | } |
oguro | 0:3e8df216e6c8 | 151 | |
oguro | 0:3e8df216e6c8 | 152 | mypwmA.write(su*Vr_adc); |
oguro | 0:3e8df216e6c8 | 153 | mypwmB.write(sv*Vr_adc); |
oguro | 0:3e8df216e6c8 | 154 | mypwmC.write(sw*Vr_adc); |
oguro | 0:3e8df216e6c8 | 155 | |
oguro | 0:3e8df216e6c8 | 156 | SWAVE=su; |
oguro | 0:3e8df216e6c8 | 157 | |
oguro | 0:3e8df216e6c8 | 158 | Speed=60*(1/(7.0*2.0*usi*1E-6)); |
oguro | 0:3e8df216e6c8 | 159 | |
oguro | 0:3e8df216e6c8 | 160 | myled = !myled; |
oguro | 0:3e8df216e6c8 | 161 | |
oguro | 0:3e8df216e6c8 | 162 | } |
oguro | 0:3e8df216e6c8 | 163 | } |
oguro | 0:3e8df216e6c8 | 164 | |
oguro | 0:3e8df216e6c8 | 165 |