Tatsuya Hiruma
gg
Dependencies: mbed MPU6050 RateLimiter test Math
Embed:
(wiki syntax)
Show/hide line numbers
main2.cpp
00001 #include "mbed.h" 00002 00003 #include "MPU6050.h" 00004 #include "x_nucleo_ihm07m1_targets.h" 00005 #include "RateLimiter.h" 00006 #include "SPN7Driver.h" 00007 00008 00009 #include <math.h> 00010 #include <stdio.h> 00011 #include <time.h> 00012 00013 00014 DigitalOut led1(LED1); 00015 AnalogIn Curr_ui(PA_0); 00016 AnalogIn Curr_vi(PC_1); 00017 AnalogIn Curr_wi(PC_0); 00018 AnalogIn V_adc(PC_2);//IHM08potentiometer 00019 AnalogOut aout(PA_4);//IHM08DAC 00020 00021 00022 MPU6050 mpu(D14,D15); 00023 //Serial pc(USBTX, USBRX); 00024 //Timer timer; 00025 00026 int diffsector,sectornow,sectorbefore = 0; 00027 int c = 0,s; 00028 float rpm; 00029 00030 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00031 // Instance of the motor driver 00032 SPN7Driver M( 00033 P_IN1, P_IN2, P_IN3, // Logic input pins 00034 P_EN1, P_EN2, P_EN3, // Enable channel pins 00035 P_HALL1, P_HALL2, P_HALL3, // Hall sensors pins 00036 P_CURR1, P_CURR2, P_CURR3, 00037 P_FAULT // Fault LED 00038 ); 00039 // Pin to check temperature on the X-NUCLEO-IHM07M1 board 00040 AnalogIn temperature(P_TEMP); 00041 00042 void checkTemperature() 00043 { 00044 if (temperature > 0.55f){ 00045 printf("Overheating... Turning off now\n\r"); 00046 M.setDutyCycle(0); 00047 M.coast(); 00048 } 00049 } 00050 /* 00051 void flip() 00052 { 00053 00054 led1 =! led1; 00055 sectornow = M.getSector(); 00056 diffsector = sectornow - sectorbefore; 00057 sectorbefore = sectornow; 00058 c = c + abs(diffsector); 00059 rpm = 2/3*abs(diffsector); 00060 00061 //printf("%f\t\r\n",rpm); 00062 00063 00064 } 00065 00066 */ 00067 00068 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00069 int main(){ 00070 //float g[3]; 00071 //int miri=1000; 00072 int accel[3],gyro[3];//accelを3つの配列で定義。 00073 float diff,theta,thetab=0,difftheta,thetavel; 00074 double a,b; 00075 double PI=3.141592; 00076 float K=0.1,T=0.005; 00077 //float P_CURR1; 00078 float dc = 0.0f; 00079 int i=0, n=1000; 00080 int diffsector,sectornow,sectorbefore = 0; 00081 //float t=0.01; 00082 float rpm ; 00083 int pole=15; 00084 Timer t ; 00085 Ticker flipper; 00086 //float CURR; 00087 float Cw,Cv,Cu; 00088 float Vr_adc; 00089 float th,thb=0; 00090 float dt =0.01; 00091 00092 float LPF_i,LPF_ib=0,LPF_th,LPF_thb=0; 00093 float F_thb=0; 00094 float ib=0,xgb=0; 00095 float e_ib=0,e_thb=0,eib=0; 00096 int gam; 00097 //float kp=0.05,ki=0.01; 00098 00099 Ticker ticker; 00100 ticker.attach(checkTemperature, 1); // Periodic overheating check 00101 /* 00102 00103 */ 00104 //led1 = 1; 00105 //flipper.attach_us(&flip,1000000.0); 00106 //flipper.attach_us(&flip,10000.0); 00107 while(1) { 00108 //wait(1); 00109 mpu.readGyroData(gyro); 00110 float xg = gyro[0]; 00111 float yg = gyro[1]; 00112 float zg = gyro[2]; 00113 00114 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入 00115 //int x = accel[0]-123;//x軸方向の加速度 00116 float xa = accel[0]; 00117 //int y = accel[1]+60;//y軸方向の加速度 00118 float ya = accel[1]; 00119 //int z = accel[2]+1110 ;//z軸方向の加速度 00120 float za = accel[2]; 00121 00122 a = ya; 00123 b = za; 00124 theta = atan2(a,b)*180/PI; 00125 00126 diff = theta - thetab; 00127 00128 00129 00130 00131 00132 00133 //printf("%d,%f,%d,%d,%d,%f\r\n",c,theta,sectornow,sectorbefore,diffsector,rpm); 00134 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00135 //theta1 = fabs(theta); 00136 //myled = 1; 00137 //wait(0.2); 00138 //myled = 0; 00139 //wait(0.2); 00140 00141 //mpu.getGyroRawZ(); 00142 //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 00143 //writing current accelerometer and gyro position 00144 //pc.printf("%d;%d;%d;%d;%d;%d\n",ax,ay,az,gx,gy,gz); 00145 00146 //printf("gyrz=%f\r\n",mpu.getAccelero()); 00147 //printf("gyrz=%f,accx=%f\r\n",mpu.getGyroRawZ(),mpu.getAcceleroRawX()); 00148 //printf("%4d,%4d,%f,%f,%f\r\n",x,y,a,b,theta); 00149 //printf("%f\r\n",theta); 00150 /* 00151 timer.reset(); 00152 timer.start(); 00153 mpu.getGyro(g); 00154 timer.stop(); 00155 float t=timer.read(); 00156 float time=t*miri; 00157 00158 pc.printf("time= %f ms\n\r",time); 00159 */ 00160 00161 00162 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00163 /* 00164 if(theta < -1 && theta > -30){ 00165 dc = (theta/30)*0.8+0.1; 00166 M.setDutyCycle(dc); 00167 }else if(theta < 30 && theta > 1){ 00168 dc = (theta/30)*0.8+0.1; 00169 M.setDutyCycle(dc); 00170 }else if(theta <= 1 && theta >= -1){ 00171 dc = 0.0f; 00172 M.setDutyCycle(dc); 00173 }else{ 00174 dc = 0.8f; 00175 M.setDutyCycle(dc); 00176 } 00177 */ 00178 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00179 //2/5 00180 00181 float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01; 00182 float alpha=0.8,beta=0.8; 00183 float e_i,e_th; 00184 float F_i,F_th; 00185 float angle; 00186 00187 //LPF_i = alpha * LPF_ib + (1 - alpha) * Curr_vi; 00188 //e_i = LPF_i - ib; 00189 //ui = Fi*ei; 00190 //LPF_th = beta * LPF_thb + (1- beta) * theta ; 00191 th = 0.05 * (thb + yg * dt) + 0.95 * theta; 00192 00193 sectornow = M.getSector(); 00194 /* 00195 diffsector = sectornow - sectorbefore; 00196 if(sector=) 00197 00198 00199 if(diffsector==5 || diffsector==-5){ 00200 diffsector=1; 00201 }else if(diffsector==4 || diffsector==-4){ 00202 diffsector=2; 00203 } 00204 00205 00206 sectorbefore = sectornow; 00207 00208 angle = diffsector*4; 00209 c = c + abs(diffsector); 00210 rpm = 2/3*abs(diffsector); 00211 */ 00212 00213 00214 00215 00216 00217 //LPF_th = LPF_th; 00218 //e_th = LPF_th-LPF_thb; 00219 e_th = th-thb; 00220 00221 if(th <= -1.0 && th> -15.0){ 00222 dc=+abs((th/70.0)+4.0/140.0); 00223 //dc=abs(th/15)+0.1; 00224 }else if(th < 15.0 && th >= 1.0){ 00225 dc=-abs((th/70.0)+4.0/140.0); 00226 //dc=-abs(th/15)-0.1; 00227 }else if(th>-1 && th<1){ 00228 dc=0.0f; 00229 //M.coast(); 00230 }else if(th < -15.0 ){ 00231 dc=0.3f; 00232 }else if(th > 15.0 ){ 00233 dc=-0.3f; 00234 }else{ 00235 dc=0.0f; 00236 } 00237 00238 //dc = (th/15.0); 00239 //dc = 0.3f; 00240 M.setDutyCycle(dc); 00241 00242 Cv = Curr_vi; 00243 00244 //printf("theta=%f\t\t,LPF_th=%f\t,th=%f\t,e_th=%f\t,dc=%f\t\r\n",theta,LPF_th,th,e_th,dc); 00245 //printf("theta=%2.4f\t,th=%2.4f\t,e_th=%2.4f\t,dc=%2.4f\t,Cv=%f\t,rpm=%f\t\r\n",theta,th,e_th,dc,Cv,rpm); 00246 printf("theta=%2.4f\t,th=%2.4f\t,e_th=%2.4f\t,dc=%2.4f\t,Cv=%f\t,%d\t,%d\t,%f\t\r\n",theta,th,e_th,dc,Cv,M.getSector(),diffsector,angle); 00247 sectorbefore = sectornow; 00248 thb=th; 00249 LPF_thb=LPF_th; 00250 00251 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00252 /* 00253 if(theta <= -4.0 && theta > -20.0){ 00254 dc = (theta/20.0)*0.5; 00255 M.setDutyCycle(dc); 00256 00257 }else if(theta < 20.0 && theta >= 4.0){ 00258 dc = (theta/20.0)*0.5; 00259 M.setDutyCycle(dc); 00260 00261 }else if(theta == 0.0){ 00262 dc = 0.1f; 00263 M.setDutyCycle(dc); 00264 00265 }else if(theta > 0.05 && theta < 4.0){ 00266 dc = K*(1-exp(-theta1/T)); 00267 M.setDutyCycle(dc); 00268 00269 }else if(theta > -4.0 && theta < -0.05){ 00270 dc = -K*(1-exp(-theta1/T)); 00271 M.setDutyCycle(dc); 00272 00273 }else{ 00274 dc = 0.5f; 00275 M.setDutyCycle(dc); 00276 } 00277 printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta); 00278 */ 00279 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00280 /* 00281 difftheta = theta1-theta ; 00282 thetavel = difftheta/0.01; 00283 00284 //dc = dc + kp*(theta - theta1)+ki*theta; 00285 00286 00287 00288 00289 if(theta <-0.5 && theta > -30.0){ 00290 //dc = (theta/30.0)*1.0; 00291 dc = kp*(theta - theta1)+ki*theta; 00292 M.setDutyCycle(dc); 00293 //if(thetavel){} 00294 00295 00296 00297 00298 }else if(theta < 30.0 && theta >0.5){ 00299 //dc = (theta/30.0)*1.0; 00300 dc = kp*(theta - theta1)+ki*theta; 00301 M.setDutyCycle(dc); 00302 00303 }else if(theta <= 0.5 && theta >= -0.5){ 00304 dc = 0.0f; 00305 M.setDutyCycle(dc); 00306 00307 }else{ 00308 dc = 0.5f; 00309 M.setDutyCycle(dc); 00310 } 00311 00312 Cw = Curr_wi.read(); 00313 Vr_adc=V_adc.read(); 00314 00315 00316 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta); 00317 //printf("dc = %f, %f ,theta=%f\r\n",dc,Cw,theta); 00318 //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta); 00319 printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel); 00320 */ 00321 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00322 //LPF 00323 /* 00324 float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01; 00325 float alpha=0.8,beta=0.8; 00326 float e_i,e_th; 00327 float F_i,F_th; 00328 00329 LPF_i = alpha * LPF_ib + (1 - alpha) * Curr_vi; 00330 e_i = LPF_i - ib; 00331 //ui = Fi*ei; 00332 00333 LPF_th = beta * LPF_thb + (1- beta) * xg/16.4 ; 00334 LPF_th = LPF_th; 00335 e_th = LPF_th-LPF_thb; 00336 00337 if(theta > -2.0 && theta < 2.0){ 00338 gam = 0; 00339 }else{ 00340 gam = 1; 00341 } 00342 00343 //if() 00344 00345 F_i = kp_i * (e_i - e_ib) + ki_i * e_i; 00346 F_th = kp_th * (e_th -e_thb) + ki_th * e_th; 00347 00348 00349 dc = gam * F_th *(LPF_th - xgb) + (1 - gam) * F_i*(LPF_i - ib); 00350 M.setDutyCycle(dc); 00351 00352 00353 //printf("xg=%f\t,%d\r\n",xg,gam); 00354 //printf("theta=%2.4f\t,dc=%1.4f\t\t\t,gam=%d\t,F_i=%f\t,F_th=%f\t,e_i=%f\t,e_th=%f\t\r\n",theta,dc,gam,F_i,F_th,e_i,e_th); 00355 printf("theta=%2.4f\t,dc=%1.4f\t\t\t,gam=%d\t,F_th=%f\t,e_th=%f\t,LPF_th=%f\t,xgb=%f\t\r\n",theta,dc,gam,F_th,e_th,LPF_th,xgb); 00356 00357 ib = Curr_vi; 00358 LPF_ib = LPF_i; 00359 LPF_thb = LPF_th; 00360 xgb = xg; 00361 */ 00362 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00363 00364 00365 00366 00367 00368 00369 00370 00371 00372 00373 00374 00375 00376 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00377 /* 00378 //test2/1 00379 float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01; 00380 float alpha=0.8,beta=0.6; 00381 float e_i,e_th,e,ei,u; 00382 float F_i,F_th,w_th; 00383 00384 th = 0.05 * (thb + yg * dt) + 0.95 * theta; 00385 //LPF_th = alpha * LPF_thb + (1- alpha) * theta ; 00386 00387 e = 0 - LPF_th; 00388 ei = eib + e * 0.01; 00389 u = kp_th * e + ki_th * ei; 00390 00391 e_th = LPF_th-LPF_thb; 00392 w_th = e_th/16.4; 00393 F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th; 00394 00395 LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ; 00396 e_i = LPF_i-LPF_ib; 00397 F_i = kp_i * (e_i -e_ib) + ki_i * e_i; 00398 if(e_th < 0.05 && e_th > -0.05){ 00399 e_th =0; 00400 } 00401 00402 if(th < 0.0 && th > -20.0){ 00403 if(e_th > -1.5 && e_th < 1.5){ 00404 if(e_th >0){ 00405 dc=th/20; 00406 M.setDutyCycle(dc); 00407 }else{ 00408 dc=-th/20; 00409 M.setDutyCycle(dc); 00410 } 00411 }else{ 00412 dc = (e_th/6.0); 00413 M.setDutyCycle(dc); 00414 } 00415 }else if(th < 20.0 && th >0.0){ 00416 if(e_th > -1.5 && e_th < 1.5){ 00417 if(e_th >0){ 00418 dc=th/20; 00419 M.setDutyCycle(dc); 00420 }else{ 00421 dc=-th/20; 00422 M.setDutyCycle(dc); 00423 } 00424 }else{ 00425 dc = (e_th/6.0); 00426 M.setDutyCycle(dc); 00427 } 00428 }else { 00429 dc = 0.5f; 00430 M.setDutyCycle(dc); 00431 } 00432 00433 eib = ei; 00434 e_thb = e_th; 00435 LPF_thb = LPF_th; 00436 thb=th; 00437 00438 Cv = Curr_vi; 00439 //Vr_adc=V_adc.read(); 00440 //printf("theta=%f\t,e=%f\t\r\n",theta,e); 00441 //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv); 00442 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta); 00443 printf("LPF_th = %f\t, theta = %f\t,e_th=%f\t,F_th=%f\t,dc=%f\t,u=%f\t\r\n",LPF_th,theta,e_th,F_th,dc,u); 00444 //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta); 00445 //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel); 00446 */ 00447 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00448 /* 00449 //test1/31 00450 float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01; 00451 float alpha=0.8,beta=0.6; 00452 float e_i,e_th; 00453 float F_i,F_th,w_th; 00454 00455 LPF_th = alpha * LPF_thb + (1- alpha) * theta ; 00456 e_th = LPF_th-LPF_thb; 00457 w_th = e_th/16.4; 00458 F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th; 00459 00460 LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ; 00461 e_i = LPF_i-LPF_ib; 00462 F_i = kp_i * (e_i -e_ib) + ki_i * e_i; 00463 00464 00465 if(LPF_th < -3.0 && LPF_th > -20.0){ 00466 if(e_th > -0.5 && e_th < 0.5){ 00467 dc = (LPF_th/20.0); 00468 M.setDutyCycle(dc); 00469 }else{ 00470 dc = abs(F_th); 00471 M.setDutyCycle(dc); 00472 } 00473 }else if(LPF_th < 20.0 && LPF_th >3.0){ 00474 if(e_th > -0.5 && e_th < 0.5){ 00475 dc = (LPF_th/20.0); 00476 M.setDutyCycle(dc); 00477 }else{ 00478 dc = -abs(F_th); 00479 M.setDutyCycle(dc); 00480 } 00481 }else if(LPF_th <= 3.0 && LPF_th > 0.0){ 00482 dc = abs(F_th); 00483 M.setDutyCycle(dc); 00484 00485 }else if(LPF_th < 0.0 && LPF_th >= -3.0){ 00486 dc = -abs(F_th); 00487 M.setDutyCycle(dc); 00488 00489 }else if(LPF_th == 0){ 00490 dc = 0.0f; 00491 M.setDutyCycle(dc); 00492 00493 }else{ 00494 dc = 0.5f; 00495 M.setDutyCycle(dc); 00496 } 00497 00498 e_thb = e_th; 00499 LPF_thb = LPF_th; 00500 00501 00502 Cv = Curr_vi; 00503 //Vr_adc=V_adc.read(); 00504 00505 //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv); 00506 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta); 00507 printf("LPF_th = %f\t, theta = %f\t,e_th=%f\t,F_th=%f\t,dc=%f\t,w=%f\t\r\n",LPF_th,theta,e_th,F_th,dc,w_th); 00508 //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta); 00509 //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel); 00510 */ 00511 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00512 00513 /* 00514 //test1/26 00515 float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.1; 00516 float alpha=0.8,beta=0.6; 00517 float e_i,e_th; 00518 float F_i,F_th; 00519 00520 LPF_th = alpha * LPF_thb + (1- alpha) * theta ; 00521 e_th = LPF_th-LPF_thb; 00522 F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th; 00523 00524 LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ; 00525 e_i = LPF_i-LPF_ib; 00526 F_i = kp_i * (e_i -e_ib) + ki_i * e_i; 00527 00528 if(LPF_th < 0 && LPF_th > -20.0){ 00529 //if(theta <0 && theta > -20.0){ 00530 //dc = (LPF_th/20.0) - F_th; 00531 dc = F_th/2; 00532 M.setDutyCycle(dc); 00533 //dc = (theta/30.0)*1.0; 00534 00535 }else if(LPF_th < 20.0 && LPF_th >0){ 00536 //dc = (LPF_th/20.0) + F_th; 00537 //dc = (theta/30.0)*1.0; 00538 dc = F_th/2; 00539 M.setDutyCycle(dc); 00540 }else if(LPF_th == 0){ 00541 //}else if(LPF_th <= 1.0 && LPF_th >= -1.0){ 00542 dc = 0.0f; 00543 M.setDutyCycle(dc); 00544 00545 }else{ 00546 dc = 0.5f; 00547 M.setDutyCycle(dc); 00548 } 00549 00550 00551 //} 00552 else{ 00553 if(theta <-4.0 && theta > -20.0){ 00554 dc = (LPF_/20.0) + F_th-0.1; 00555 M.setDutyCycle(dc); 00556 //dc = (theta/30.0)*1.0; 00557 00558 }else if(LPF_th < 30.0 && LPF_th >4.0){ 00559 dc = (LPF_th/20.0) + F_th+0.1; 00560 //dc = (theta/30.0)*1.0; 00561 M.setDutyCycle(dc); 00562 00563 }else if(LPF_th <= 0.5 && LPF_th >= -0.5){ 00564 dc = 0.0f; 00565 M.setDutyCycle(dc); 00566 00567 }else{ 00568 dc = 0.5f; 00569 M.setDutyCycle(dc); 00570 } 00571 } 00572 00573 00574 e_thb = e_th; 00575 LPF_thb = LPF_th; 00576 00577 00578 Cv = Curr_vi; 00579 //Vr_adc=V_adc.read(); 00580 00581 //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv); 00582 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta); 00583 printf("LPF_th = %f\t, theta = %f\t,e_th=%f\t,F_th=%f\t,dc=%f\t\r\n",LPF_th,theta,e_th,F_th,dc); 00584 //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta); 00585 //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel); 00586 */ 00587 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00588 /* 00589 //ok 00590 if(theta <-0.5 && theta > -30.0){ 00591 dc = (theta/30.0)*1.0; 00592 M.setDutyCycle(dc); 00593 00594 }else if(theta < 30.0 && theta >0.5){ 00595 dc = (theta/30.0)*1.0; 00596 M.setDutyCycle(dc); 00597 00598 }else if(theta <= 0.5 && theta >= -0.5){ 00599 dc = 0.0f; 00600 M.setDutyCycle(dc); 00601 00602 }else{ 00603 dc = 0.5f; 00604 M.setDutyCycle(dc); 00605 } 00606 00607 Cv = Curr_vi; 00608 //Vr_adc=V_adc.read(); 00609 00610 00611 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta); 00612 printf("dc = %f, %f ,theta=%f\r\n",dc,Cv,theta); 00613 //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta); 00614 //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel); 00615 */ 00616 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00617 /* 00618 t.start(); 00619 sectornow = M.getSector(); 00620 diffsector = sectornow - sectorbefore; 00621 if(diffsector > 0 || abs(diffsector) == 5){ 00622 //t.stop(); 00623 rpm=60.0/t; 00624 }else{ 00625 //t.stop(); 00626 rpm=0.0; 00627 } 00628 printf("dc=%f,%d,time=%d,rpm=%f,%d\r\n",dc,M.getSector(),t.read_ms(),rpm,diffsector); 00629 //t.reset(); 00630 00631 00632 00633 //if(diff<20){ 00634 00635 */ 00636 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00637 00638 00639 00640 00641 00642 00643 00644 00645 00646 00647 00648 00649 00650 00651 00652 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00653 /*//duty test1 00654 char c = getchar(); 00655 if((c == 'w') && (dc < 0.9f)) { 00656 dc += 0.1f; 00657 M.setDutyCycle(dc); 00658 } 00659 if((c == 's') && (dc > -0.9f)) { 00660 dc -= 0.1f; 00661 M.setDutyCycle(dc); 00662 } 00663 */ 00664 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00665 //duty test2 00666 /* 00667 s = getsector(); 00668 diffsector = sectornow - sectorbefore; 00669 count = count + diffsector; 00670 00671 00672 00673 sectorbefore = sectornow; 00674 00675 } 00676 00677 dc = 0.1; 00678 M.setDutyCycle(dc); 00679 00680 00681 00682 rpm = 90*sector*60/t; 00683 00684 00685 printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),diff); 00686 */ 00687 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00688 /* 00689 if(abs(diff)<20 && abs(diff)>0){ 00690 00691 dc = diff/20 * 0.5; 00692 00693 }else{ 00694 00695 dc = 0.0; 00696 00697 } 00698 */ 00699 00700 00701 00702 00703 00704 00705 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),diff); 00706 //printf("dc=%f,%d,time=%d,rpm=%f,%d\r\n",dc,M.getSector(),t.read_us(),rpm,diffsector); 00707 //////////////////////////////////////////////////////////////////////////////////////////////////////////// 00708 00709 00710 00711 00712 00713 thetab = theta; 00714 //sectorbefore = sectornow; 00715 i++; 00716 } 00717 00718 }
Generated on Tue Jul 26 2022 18:29:46 by
1.7.2