akiyoshi oguro / Mbed 2 deprecated Vector_Open_sin

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mbed-rtos

main.cpp@2:0146f9f7701c, 2019-06-08 (annotated)

Committer:
oguro
Date:
Sat Jun 08 03:02:33 2019 +0000
Revision:
2:0146f9f7701c
Parent:
1:ce18261eee75 
Open Vector sin

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oguro 0:93a9ccc6b7e2 1 #include "mbed.h"
oguro 0:93a9ccc6b7e2 2 #include "rtos.h"
oguro 0:93a9ccc6b7e2 3 //#include "adc.h"
oguro 0:93a9ccc6b7e2 4 #include <math.h>
oguro 0:93a9ccc6b7e2 5 #define TS1 0.2
oguro 0:93a9ccc6b7e2 6
oguro 0:93a9ccc6b7e2 7 float PI=3.141592;
oguro 0:93a9ccc6b7e2 8 int t=0,q=0,i=0;
oguro 0:93a9ccc6b7e2 9 int START=7;
oguro 1:ce18261eee75 10 PwmOut mypwmA(PA_8);
oguro 1:ce18261eee75 11 PwmOut mypwmB(PA_9);
oguro 1:ce18261eee75 12 PwmOut mypwmC(PA_10);
oguro 0:93a9ccc6b7e2 13
oguro 0:93a9ccc6b7e2 14 PwmOut Current_Ref(PB_4);
oguro 0:93a9ccc6b7e2 15
oguro 0:93a9ccc6b7e2 16 DigitalOut Vector(PC_4);
oguro 0:93a9ccc6b7e2 17 DigitalOut EN1(PC_10);
oguro 0:93a9ccc6b7e2 18 DigitalOut EN2(PC_11);
oguro 0:93a9ccc6b7e2 19 DigitalOut EN3(PC_12);
oguro 0:93a9ccc6b7e2 20
oguro 0:93a9ccc6b7e2 21
oguro 0:93a9ccc6b7e2 22
oguro 0:93a9ccc6b7e2 23 //AnalogIn V_adc(PB_1); //volume
oguro 0:93a9ccc6b7e2 24 AnalogIn V_adc(PC_2); // Gaibu Potention
oguro 0:93a9ccc6b7e2 25
oguro 0:93a9ccc6b7e2 26 AnalogIn Vshuntu(PA_1);
oguro 0:93a9ccc6b7e2 27 AnalogIn Vshuntv(PA_0);
oguro 0:93a9ccc6b7e2 28 AnalogIn Vshuntw(PB_0);
oguro 0:93a9ccc6b7e2 29
oguro 0:93a9ccc6b7e2 30 AnalogIn BEMF1(PC_3);//C7_37
oguro 0:93a9ccc6b7e2 31 AnalogIn BEMF2(PB_0);//C7_34
oguro 0:93a9ccc6b7e2 32 AnalogIn BEMF3(PA_7);//C10_26
oguro 0:93a9ccc6b7e2 33
oguro 0:93a9ccc6b7e2 34 InterruptIn HA(PA_15);
oguro 0:93a9ccc6b7e2 35 InterruptIn HB(PB_3);
oguro 0:93a9ccc6b7e2 36 InterruptIn HC(PB_10);
oguro 0:93a9ccc6b7e2 37 Timer uT;
oguro 0:93a9ccc6b7e2 38 Timer vT;
oguro 0:93a9ccc6b7e2 39 Timer wT;
oguro 0:93a9ccc6b7e2 40
oguro 0:93a9ccc6b7e2 41 //AnalogIn VBUS(PA_1);
oguro 0:93a9ccc6b7e2 42
oguro 0:93a9ccc6b7e2 43 DigitalIn GPIO_BEMF(PC_9);
oguro 0:93a9ccc6b7e2 44 DigitalIn CPOUT(PA_12);
oguro 0:93a9ccc6b7e2 45 AnalogIn Curr_ui(PA_0);
oguro 0:93a9ccc6b7e2 46 AnalogIn Curr_vi(PC_1);
oguro 0:93a9ccc6b7e2 47 AnalogIn Curr_wi(PA_1);
oguro 0:93a9ccc6b7e2 48
oguro 0:93a9ccc6b7e2 49 AnalogOut SWAVE(PA_4);
oguro 0:93a9ccc6b7e2 50
oguro 0:93a9ccc6b7e2 51 Serial pc(USBTX,USBRX);
oguro 0:93a9ccc6b7e2 52
oguro 0:93a9ccc6b7e2 53 DigitalOut myled(LED1);
oguro 0:93a9ccc6b7e2 54 float sq32=sqrt(3.0f/2.0f);
oguro 0:93a9ccc6b7e2 55 float sq23=2.0f/sqrt(3.0f);
oguro 0:93a9ccc6b7e2 56 float sq3=1.0f/sqrt(3.0f);
oguro 0:93a9ccc6b7e2 57 float Vdlink=3.0f/5.0f;
oguro 0:93a9ccc6b7e2 58 float aVa;
oguro 0:93a9ccc6b7e2 59 float a3Vb;
oguro 0:93a9ccc6b7e2 60 float Vr_adc;
oguro 0:93a9ccc6b7e2 61
oguro 0:93a9ccc6b7e2 62 Timer Timer1;
oguro 0:93a9ccc6b7e2 63 float freq=0;
oguro 0:93a9ccc6b7e2 64 float z=0.5;
oguro 0:93a9ccc6b7e2 65 float Wt,Va,Vb,Vq,Vd;
oguro 0:93a9ccc6b7e2 66 float d1,d2,d3,d4,d5,d6,d07;
oguro 0:93a9ccc6b7e2 67 float du,dv,dw;
oguro 0:93a9ccc6b7e2 68 bool cu,cv,cw;
oguro 0:93a9ccc6b7e2 69 float ave=0,aveo=0,avsum=0;
oguro 0:93a9ccc6b7e2 70 float zet=sqrt(2.0f/3.0f),cos23=cos((2.0f/3.0f)*PI);
oguro 0:93a9ccc6b7e2 71 float cos43=cos((4.0f/3.0f)*PI),sin23=sin((2.0f/3.0f)*PI),sin43=sin((4.0f/3.0f)*PI);
oguro 0:93a9ccc6b7e2 72
oguro 0:93a9ccc6b7e2 73 float ut=0,ut1=0,ut2=0,usi=0;
oguro 0:93a9ccc6b7e2 74 float vt=0,vt1=0,vt2=0,vsi=0;
oguro 0:93a9ccc6b7e2 75 float wt=0,wt1=0,wt2=0,wsi=0;
oguro 0:93a9ccc6b7e2 76 float Speed;
oguro 0:93a9ccc6b7e2 77
oguro 0:93a9ccc6b7e2 78 void HAH(){
oguro 0:93a9ccc6b7e2 79
oguro 0:93a9ccc6b7e2 80 ut1=uT.read_us();
oguro 0:93a9ccc6b7e2 81 ut=0;
oguro 0:93a9ccc6b7e2 82
oguro 0:93a9ccc6b7e2 83 }
oguro 0:93a9ccc6b7e2 84 void HAL(){
oguro 0:93a9ccc6b7e2 85
oguro 0:93a9ccc6b7e2 86 ut2=uT.read_us();
oguro 0:93a9ccc6b7e2 87 uT.reset();
oguro 0:93a9ccc6b7e2 88
oguro 0:93a9ccc6b7e2 89 }
oguro 0:93a9ccc6b7e2 90 void HBH(){
oguro 0:93a9ccc6b7e2 91
oguro 0:93a9ccc6b7e2 92 vt1=vT.read_us();
oguro 0:93a9ccc6b7e2 93
oguro 0:93a9ccc6b7e2 94 }
oguro 0:93a9ccc6b7e2 95 void HBL(){
oguro 0:93a9ccc6b7e2 96
oguro 0:93a9ccc6b7e2 97 vt2=vT.read_us();
oguro 0:93a9ccc6b7e2 98
oguro 0:93a9ccc6b7e2 99 vT.reset();
oguro 0:93a9ccc6b7e2 100 }
oguro 0:93a9ccc6b7e2 101 void HCH(){
oguro 0:93a9ccc6b7e2 102 wt1=wT.read_us();
oguro 0:93a9ccc6b7e2 103
oguro 0:93a9ccc6b7e2 104 }
oguro 0:93a9ccc6b7e2 105
oguro 0:93a9ccc6b7e2 106 void HCL(){
oguro 0:93a9ccc6b7e2 107 wt2=wT.read_us();
oguro 0:93a9ccc6b7e2 108
oguro 0:93a9ccc6b7e2 109 wT.reset();
oguro 0:93a9ccc6b7e2 110 }
oguro 0:93a9ccc6b7e2 111
oguro 0:93a9ccc6b7e2 112
oguro 0:93a9ccc6b7e2 113
oguro 0:93a9ccc6b7e2 114 void CPLT(){
oguro 0:93a9ccc6b7e2 115 pc.printf("%.3f , %.3f \r" ,Speed ,Vr_adc);
oguro 0:93a9ccc6b7e2 116 }
oguro 0:93a9ccc6b7e2 117
oguro 0:93a9ccc6b7e2 118 void timerTS1(void const*argument){
oguro 0:93a9ccc6b7e2 119 CPLT();
oguro 0:93a9ccc6b7e2 120 }
oguro 0:93a9ccc6b7e2 121
oguro 0:93a9ccc6b7e2 122 int main() {
oguro 0:93a9ccc6b7e2 123
oguro 0:93a9ccc6b7e2 124 Timer1.start();
oguro 0:93a9ccc6b7e2 125 EN1=1;
oguro 0:93a9ccc6b7e2 126 EN2=1;
oguro 0:93a9ccc6b7e2 127 EN3=1;
oguro 0:93a9ccc6b7e2 128
oguro 0:93a9ccc6b7e2 129 uT.start();
oguro 0:93a9ccc6b7e2 130 vT.start();
oguro 0:93a9ccc6b7e2 131 wT.start();
oguro 0:93a9ccc6b7e2 132
oguro 0:93a9ccc6b7e2 133 mypwmA.period_us(20);
oguro 0:93a9ccc6b7e2 134
oguro 0:93a9ccc6b7e2 135 mypwmB.period_us(20);
oguro 0:93a9ccc6b7e2 136
oguro 0:93a9ccc6b7e2 137 mypwmC.period_us(20);
oguro 0:93a9ccc6b7e2 138
oguro 0:93a9ccc6b7e2 139 pc.baud(128000);
oguro 0:93a9ccc6b7e2 140 RtosTimer RtosTimerTS1(timerTS1);
oguro 0:93a9ccc6b7e2 141 RtosTimerTS1.start((unsigned int)(TS1*5000)); //3000
oguro 0:93a9ccc6b7e2 142 Thread::wait(100);
oguro 0:93a9ccc6b7e2 143
oguro 0:93a9ccc6b7e2 144 Vq=0.7;
oguro 0:93a9ccc6b7e2 145 Vd=0;
oguro 0:93a9ccc6b7e2 146
oguro 0:93a9ccc6b7e2 147 while(1) {
oguro 0:93a9ccc6b7e2 148
oguro 0:93a9ccc6b7e2 149
oguro 0:93a9ccc6b7e2 150
oguro 0:93a9ccc6b7e2 151
oguro 0:93a9ccc6b7e2 152 HA.rise(&HAH);
oguro 0:93a9ccc6b7e2 153 HC.fall(&HCL);
oguro 0:93a9ccc6b7e2 154 HB.rise(&HBH);
oguro 0:93a9ccc6b7e2 155 HA.fall(&HAL);
oguro 0:93a9ccc6b7e2 156 HC.rise(&HCH);
oguro 0:93a9ccc6b7e2 157 HB.fall(&HBL);
oguro 0:93a9ccc6b7e2 158 Speed=60*(1/(7.0*2.0*usi*1E-6));
oguro 0:93a9ccc6b7e2 159 usi=ut2-ut1;
oguro 0:93a9ccc6b7e2 160 vsi=vt2-vt1;
oguro 0:93a9ccc6b7e2 161 wsi=wt2-wt1;
oguro 0:93a9ccc6b7e2 162
oguro 0:93a9ccc6b7e2 163 float s = Timer1.read();
oguro 0:93a9ccc6b7e2 164 Vr_adc=V_adc.read();
oguro 0:93a9ccc6b7e2 165 myled = ((int)s) % 2 == 0; /* LED output(0.5Hz) */
oguro 0:93a9ccc6b7e2 166 if((Vr_adc>0.08f)&&(q==0)){
oguro 0:93a9ccc6b7e2 167
oguro 0:93a9ccc6b7e2 168 while(q<50){
oguro 0:93a9ccc6b7e2 169
oguro 0:93a9ccc6b7e2 170 mypwmA.write(0.5f);
oguro 0:93a9ccc6b7e2 171 mypwmB.write(0);
oguro 0:93a9ccc6b7e2 172 mypwmC.write(0);
oguro 0:93a9ccc6b7e2 173 wait_ms(START);
oguro 0:93a9ccc6b7e2 174
oguro 0:93a9ccc6b7e2 175 mypwmA.write(0);
oguro 0:93a9ccc6b7e2 176 mypwmB.write(0.5f);
oguro 0:93a9ccc6b7e2 177 mypwmC.write(0.0);
oguro 0:93a9ccc6b7e2 178 wait_ms(START);
oguro 0:93a9ccc6b7e2 179
oguro 0:93a9ccc6b7e2 180 mypwmA.write(0);
oguro 0:93a9ccc6b7e2 181 mypwmB.write(0.0);
oguro 0:93a9ccc6b7e2 182 mypwmC.write(0.5f);
oguro 0:93a9ccc6b7e2 183 wait_ms(START);
oguro 0:93a9ccc6b7e2 184 q++;
oguro 0:93a9ccc6b7e2 185
oguro 0:93a9ccc6b7e2 186 }
oguro 0:93a9ccc6b7e2 187 }
oguro 0:93a9ccc6b7e2 188 if(Vr_adc < 0.05f){
oguro 0:93a9ccc6b7e2 189 q=0;
oguro 0:93a9ccc6b7e2 190 Vr_adc=0;
oguro 0:93a9ccc6b7e2 191
oguro 0:93a9ccc6b7e2 192 }
oguro 0:93a9ccc6b7e2 193
oguro 0:93a9ccc6b7e2 194 aveo=Vr_adc;
oguro 0:93a9ccc6b7e2 195
oguro 0:93a9ccc6b7e2 196 if((aveo<=0.15)&&(aveo>0.1)){
oguro 0:93a9ccc6b7e2 197 Wt=fmodf(s * 50, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 198 Vq=0.4;
oguro 0:93a9ccc6b7e2 199 }
oguro 0:93a9ccc6b7e2 200 if((aveo<=0.2)&&(aveo>0.15)){
oguro 0:93a9ccc6b7e2 201 Wt=fmodf(s * 55, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 202 Vq=0.45;
oguro 0:93a9ccc6b7e2 203 }
oguro 0:93a9ccc6b7e2 204 if((aveo<=0.25)&&(aveo>0.2)){
oguro 0:93a9ccc6b7e2 205 Wt=fmodf(s * 60, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 206 Vq=0.5;
oguro 0:93a9ccc6b7e2 207 }
oguro 0:93a9ccc6b7e2 208 if((aveo<=0.3)&&(aveo>0.25)){
oguro 0:93a9ccc6b7e2 209 Wt=fmodf(s * 65, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 210 Vq=0.55;
oguro 0:93a9ccc6b7e2 211 }
oguro 0:93a9ccc6b7e2 212 if((aveo<=0.35)&&(aveo>0.3)){
oguro 0:93a9ccc6b7e2 213 Wt=fmodf(s * 70, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 214 Vq=0.6;
oguro 0:93a9ccc6b7e2 215 }
oguro 0:93a9ccc6b7e2 216 if((aveo<=0.4)&&(aveo>0.35)){
oguro 0:93a9ccc6b7e2 217 Wt=fmodf(s * 80, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 218 Vq=0.65;
oguro 0:93a9ccc6b7e2 219 }
oguro 0:93a9ccc6b7e2 220 if((aveo<=0.45)&&(aveo>0.4)){
oguro 0:93a9ccc6b7e2 221 Wt=fmodf(s * 90, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 222 Vq=0.7;
oguro 0:93a9ccc6b7e2 223 }
oguro 0:93a9ccc6b7e2 224 if((aveo<=0.5)&&(aveo>0.45)){
oguro 0:93a9ccc6b7e2 225 Wt=fmodf(s * 100, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 226 Vq=0.7;
oguro 0:93a9ccc6b7e2 227 }
oguro 0:93a9ccc6b7e2 228 if((aveo<=0.55)&&(aveo>0.5)){
oguro 0:93a9ccc6b7e2 229 Wt=fmodf(s * 110, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 230 Vq=0.7;
oguro 0:93a9ccc6b7e2 231 }
oguro 0:93a9ccc6b7e2 232 if((aveo<=0.6)&&(aveo>0.55)){
oguro 0:93a9ccc6b7e2 233 Wt=fmodf(s * 120, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 234 Vq=0.7;
oguro 0:93a9ccc6b7e2 235 }
oguro 0:93a9ccc6b7e2 236 if((aveo<=0.65)&&(aveo>0.6)){
oguro 0:93a9ccc6b7e2 237 Wt=fmodf(s * 130, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 238 Vq=0.8;
oguro 0:93a9ccc6b7e2 239 }
oguro 0:93a9ccc6b7e2 240 if((aveo<=0.7)&&(aveo>0.65)){
oguro 0:93a9ccc6b7e2 241 Wt=fmodf(s * 140, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 242 Vq=0.8;
oguro 0:93a9ccc6b7e2 243 }
oguro 0:93a9ccc6b7e2 244 if((aveo<=0.75)&&(aveo>0.7)){
oguro 0:93a9ccc6b7e2 245 Wt=fmodf(s * 150, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 246 Vq=0.8;
oguro 0:93a9ccc6b7e2 247 }
oguro 0:93a9ccc6b7e2 248 if((aveo<=0.8)&&(aveo>0.75)){
oguro 0:93a9ccc6b7e2 249 Wt=fmodf(s * 160, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 250 Vq=0.8;
oguro 0:93a9ccc6b7e2 251 }
oguro 0:93a9ccc6b7e2 252 if((aveo<=0.85)&&(aveo>0.8)){
oguro 0:93a9ccc6b7e2 253 Wt=fmodf(s * 170, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 254 Vq=0.9;
oguro 0:93a9ccc6b7e2 255 }
oguro 0:93a9ccc6b7e2 256 if((aveo<=0.9)&&(aveo>0.85)){
oguro 0:93a9ccc6b7e2 257 Wt=fmodf(s * 180, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 258 Vq=0.9;
oguro 0:93a9ccc6b7e2 259 }
oguro 0:93a9ccc6b7e2 260 if((aveo<=0.95)&&(aveo>0.9)){
oguro 0:93a9ccc6b7e2 261 Wt=fmodf(s * 190, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 262 Vq=0.9;
oguro 0:93a9ccc6b7e2 263 }
oguro 0:93a9ccc6b7e2 264 if(aveo>0.95){
oguro 0:93a9ccc6b7e2 265 Wt=fmodf(s * 200, 1) * PI* 2;
oguro 0:93a9ccc6b7e2 266 Vq=0.9;
oguro 0:93a9ccc6b7e2 267 }
oguro 0:93a9ccc6b7e2 268
oguro 0:93a9ccc6b7e2 269 if(aveo<0.1){
oguro 0:93a9ccc6b7e2 270 mypwmA.write(0);
oguro 0:93a9ccc6b7e2 271 mypwmB.write(0);
oguro 0:93a9ccc6b7e2 272 mypwmC.write(0);
oguro 0:93a9ccc6b7e2 273
oguro 0:93a9ccc6b7e2 274 }
oguro 0:93a9ccc6b7e2 275 else{
oguro 0:93a9ccc6b7e2 276 Va=cos(Wt)*Vd-sin(Wt)*Vq;
oguro 0:93a9ccc6b7e2 277 Vb=sin(Wt)*Vd+cos(Wt)*Vq;
oguro 0:93a9ccc6b7e2 278
oguro 2:0146f9f7701c 279 du=(Va*zet+0.5);
oguro 2:0146f9f7701c 280 dv=((Va*cos23+Vb*sin23)*zet+0.5);
oguro 2:0146f9f7701c 281 dw=((Va*cos43+Vb*sin43)*zet+0.5);
oguro 0:93a9ccc6b7e2 282
oguro 0:93a9ccc6b7e2 283 mypwmA.write(du);
oguro 0:93a9ccc6b7e2 284 mypwmB.write(dv);
oguro 0:93a9ccc6b7e2 285 mypwmC.write(dw);
oguro 0:93a9ccc6b7e2 286
oguro 0:93a9ccc6b7e2 287 SWAVE=dw;
oguro 0:93a9ccc6b7e2 288
oguro 0:93a9ccc6b7e2 289 }
oguro 0:93a9ccc6b7e2 290 }
oguro 0:93a9ccc6b7e2 291 }