Tatsuya Hiruma
gg
Dependencies: mbed MPU6050 RateLimiter test Math
Diff: main2.cpp
- Revision:
- 8:7efca5258efb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main2.cpp Fri Mar 06 05:58:45 2020 +0000
@@ -0,0 +1,718 @@
+#include "mbed.h"
+
+#include "MPU6050.h"
+#include "x_nucleo_ihm07m1_targets.h"
+#include "RateLimiter.h"
+#include "SPN7Driver.h"
+
+
+#include <math.h>
+#include <stdio.h>
+#include <time.h>
+
+
+DigitalOut led1(LED1);
+AnalogIn Curr_ui(PA_0);
+AnalogIn Curr_vi(PC_1);
+AnalogIn Curr_wi(PC_0);
+AnalogIn V_adc(PC_2);//IHM08potentiometer
+AnalogOut aout(PA_4);//IHM08DAC
+
+
+MPU6050 mpu(D14,D15);
+//Serial pc(USBTX, USBRX);
+//Timer timer;
+
+ int diffsector,sectornow,sectorbefore = 0;
+ int c = 0,s;
+ float rpm;
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Instance of the motor driver
+SPN7Driver M(
+ P_IN1, P_IN2, P_IN3, // Logic input pins
+ P_EN1, P_EN2, P_EN3, // Enable channel pins
+ P_HALL1, P_HALL2, P_HALL3, // Hall sensors pins
+ P_CURR1, P_CURR2, P_CURR3,
+ P_FAULT // Fault LED
+ );
+// Pin to check temperature on the X-NUCLEO-IHM07M1 board
+AnalogIn temperature(P_TEMP);
+
+void checkTemperature()
+{
+ if (temperature > 0.55f){
+ printf("Overheating... Turning off now\n\r");
+ M.setDutyCycle(0);
+ M.coast();
+ }
+}
+/*
+void flip()
+ {
+
+ led1 =! led1;
+ sectornow = M.getSector();
+ diffsector = sectornow - sectorbefore;
+ sectorbefore = sectornow;
+ c = c + abs(diffsector);
+ rpm = 2/3*abs(diffsector);
+
+ //printf("%f\t\r\n",rpm);
+
+
+ }
+
+*/
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+int main(){
+ //float g[3];
+ //int miri=1000;
+ int accel[3],gyro[3];//accelを3つの配列で定義。
+ float diff,theta,thetab=0,difftheta,thetavel;
+ double a,b;
+ double PI=3.141592;
+ float K=0.1,T=0.005;
+ //float P_CURR1;
+ float dc = 0.0f;
+ int i=0, n=1000;
+ int diffsector,sectornow,sectorbefore = 0;
+ //float t=0.01;
+ float rpm ;
+ int pole=15;
+ Timer t ;
+ Ticker flipper;
+ //float CURR;
+ float Cw,Cv,Cu;
+ float Vr_adc;
+ float th,thb=0;
+ float dt =0.01;
+
+ float LPF_i,LPF_ib=0,LPF_th,LPF_thb=0;
+ float F_thb=0;
+ float ib=0,xgb=0;
+ float e_ib=0,e_thb=0,eib=0;
+ int gam;
+ //float kp=0.05,ki=0.01;
+
+ Ticker ticker;
+ ticker.attach(checkTemperature, 1); // Periodic overheating check
+ /*
+
+ */
+ //led1 = 1;
+ //flipper.attach_us(&flip,1000000.0);
+ //flipper.attach_us(&flip,10000.0);
+ while(1) {
+ //wait(1);
+ mpu.readGyroData(gyro);
+ float xg = gyro[0];
+ float yg = gyro[1];
+ float zg = gyro[2];
+
+ mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
+ //int x = accel[0]-123;//x軸方向の加速度
+ float xa = accel[0];
+ //int y = accel[1]+60;//y軸方向の加速度
+ float ya = accel[1];
+ //int z = accel[2]+1110 ;//z軸方向の加速度
+ float za = accel[2];
+
+ a = ya;
+ b = za;
+ theta = atan2(a,b)*180/PI;
+
+ diff = theta - thetab;
+
+
+
+
+
+
+ //printf("%d,%f,%d,%d,%d,%f\r\n",c,theta,sectornow,sectorbefore,diffsector,rpm);
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //theta1 = fabs(theta);
+ //myled = 1;
+ //wait(0.2);
+ //myled = 0;
+ //wait(0.2);
+
+ //mpu.getGyroRawZ();
+ //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+ //writing current accelerometer and gyro position
+ //pc.printf("%d;%d;%d;%d;%d;%d\n",ax,ay,az,gx,gy,gz);
+
+ //printf("gyrz=%f\r\n",mpu.getAccelero());
+ //printf("gyrz=%f,accx=%f\r\n",mpu.getGyroRawZ(),mpu.getAcceleroRawX());
+ //printf("%4d,%4d,%f,%f,%f\r\n",x,y,a,b,theta);
+ //printf("%f\r\n",theta);
+ /*
+ timer.reset();
+ timer.start();
+ mpu.getGyro(g);
+ timer.stop();
+ float t=timer.read();
+ float time=t*miri;
+
+ pc.printf("time= %f ms\n\r",time);
+ */
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ if(theta < -1 && theta > -30){
+ dc = (theta/30)*0.8+0.1;
+ M.setDutyCycle(dc);
+ }else if(theta < 30 && theta > 1){
+ dc = (theta/30)*0.8+0.1;
+ M.setDutyCycle(dc);
+ }else if(theta <= 1 && theta >= -1){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+ }else{
+ dc = 0.8f;
+ M.setDutyCycle(dc);
+ }
+ */
+ ////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //2/5
+
+ float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
+ float alpha=0.8,beta=0.8;
+ float e_i,e_th;
+ float F_i,F_th;
+ float angle;
+
+ //LPF_i = alpha * LPF_ib + (1 - alpha) * Curr_vi;
+ //e_i = LPF_i - ib;
+ //ui = Fi*ei;
+ //LPF_th = beta * LPF_thb + (1- beta) * theta ;
+ th = 0.05 * (thb + yg * dt) + 0.95 * theta;
+
+ sectornow = M.getSector();
+ /*
+ diffsector = sectornow - sectorbefore;
+ if(sector=)
+
+
+ if(diffsector==5 || diffsector==-5){
+ diffsector=1;
+ }else if(diffsector==4 || diffsector==-4){
+ diffsector=2;
+ }
+
+
+ sectorbefore = sectornow;
+
+ angle = diffsector*4;
+ c = c + abs(diffsector);
+ rpm = 2/3*abs(diffsector);
+ */
+
+
+
+
+
+ //LPF_th = LPF_th;
+ //e_th = LPF_th-LPF_thb;
+ e_th = th-thb;
+
+ if(th <= -1.0 && th> -15.0){
+ dc=+abs((th/70.0)+4.0/140.0);
+ //dc=abs(th/15)+0.1;
+ }else if(th < 15.0 && th >= 1.0){
+ dc=-abs((th/70.0)+4.0/140.0);
+ //dc=-abs(th/15)-0.1;
+ }else if(th>-1 && th<1){
+ dc=0.0f;
+ //M.coast();
+ }else if(th < -15.0 ){
+ dc=0.3f;
+ }else if(th > 15.0 ){
+ dc=-0.3f;
+ }else{
+ dc=0.0f;
+ }
+
+ //dc = (th/15.0);
+ //dc = 0.3f;
+ M.setDutyCycle(dc);
+
+ Cv = Curr_vi;
+
+ //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);
+ //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);
+ 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);
+ sectorbefore = sectornow;
+ thb=th;
+ LPF_thb=LPF_th;
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ if(theta <= -4.0 && theta > -20.0){
+ dc = (theta/20.0)*0.5;
+ M.setDutyCycle(dc);
+
+ }else if(theta < 20.0 && theta >= 4.0){
+ dc = (theta/20.0)*0.5;
+ M.setDutyCycle(dc);
+
+ }else if(theta == 0.0){
+ dc = 0.1f;
+ M.setDutyCycle(dc);
+
+ }else if(theta > 0.05 && theta < 4.0){
+ dc = K*(1-exp(-theta1/T));
+ M.setDutyCycle(dc);
+
+ }else if(theta > -4.0 && theta < -0.05){
+ dc = -K*(1-exp(-theta1/T));
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+ printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ difftheta = theta1-theta ;
+ thetavel = difftheta/0.01;
+
+ //dc = dc + kp*(theta - theta1)+ki*theta;
+
+
+
+
+ if(theta <-0.5 && theta > -30.0){
+ //dc = (theta/30.0)*1.0;
+ dc = kp*(theta - theta1)+ki*theta;
+ M.setDutyCycle(dc);
+ //if(thetavel){}
+
+
+
+
+ }else if(theta < 30.0 && theta >0.5){
+ //dc = (theta/30.0)*1.0;
+ dc = kp*(theta - theta1)+ki*theta;
+ M.setDutyCycle(dc);
+
+ }else if(theta <= 0.5 && theta >= -0.5){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+
+ Cw = Curr_wi.read();
+ Vr_adc=V_adc.read();
+
+
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ //printf("dc = %f, %f ,theta=%f\r\n",dc,Cw,theta);
+ //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
+ printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //LPF
+ /*
+ float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
+ float alpha=0.8,beta=0.8;
+ float e_i,e_th;
+ float F_i,F_th;
+
+ LPF_i = alpha * LPF_ib + (1 - alpha) * Curr_vi;
+ e_i = LPF_i - ib;
+ //ui = Fi*ei;
+
+ LPF_th = beta * LPF_thb + (1- beta) * xg/16.4 ;
+ LPF_th = LPF_th;
+ e_th = LPF_th-LPF_thb;
+
+ if(theta > -2.0 && theta < 2.0){
+ gam = 0;
+ }else{
+ gam = 1;
+ }
+
+ //if()
+
+ F_i = kp_i * (e_i - e_ib) + ki_i * e_i;
+ F_th = kp_th * (e_th -e_thb) + ki_th * e_th;
+
+
+ dc = gam * F_th *(LPF_th - xgb) + (1 - gam) * F_i*(LPF_i - ib);
+ M.setDutyCycle(dc);
+
+
+ //printf("xg=%f\t,%d\r\n",xg,gam);
+ //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);
+ 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);
+
+ ib = Curr_vi;
+ LPF_ib = LPF_i;
+ LPF_thb = LPF_th;
+ xgb = xg;
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+
+
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ //test2/1
+ float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
+ float alpha=0.8,beta=0.6;
+ float e_i,e_th,e,ei,u;
+ float F_i,F_th,w_th;
+
+ th = 0.05 * (thb + yg * dt) + 0.95 * theta;
+ //LPF_th = alpha * LPF_thb + (1- alpha) * theta ;
+
+ e = 0 - LPF_th;
+ ei = eib + e * 0.01;
+ u = kp_th * e + ki_th * ei;
+
+ e_th = LPF_th-LPF_thb;
+ w_th = e_th/16.4;
+ F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th;
+
+ LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ;
+ e_i = LPF_i-LPF_ib;
+ F_i = kp_i * (e_i -e_ib) + ki_i * e_i;
+ if(e_th < 0.05 && e_th > -0.05){
+ e_th =0;
+ }
+
+ if(th < 0.0 && th > -20.0){
+ if(e_th > -1.5 && e_th < 1.5){
+ if(e_th >0){
+ dc=th/20;
+ M.setDutyCycle(dc);
+ }else{
+ dc=-th/20;
+ M.setDutyCycle(dc);
+ }
+ }else{
+ dc = (e_th/6.0);
+ M.setDutyCycle(dc);
+ }
+ }else if(th < 20.0 && th >0.0){
+ if(e_th > -1.5 && e_th < 1.5){
+ if(e_th >0){
+ dc=th/20;
+ M.setDutyCycle(dc);
+ }else{
+ dc=-th/20;
+ M.setDutyCycle(dc);
+ }
+ }else{
+ dc = (e_th/6.0);
+ M.setDutyCycle(dc);
+ }
+ }else {
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+
+ eib = ei;
+ e_thb = e_th;
+ LPF_thb = LPF_th;
+ thb=th;
+
+ Cv = Curr_vi;
+ //Vr_adc=V_adc.read();
+ //printf("theta=%f\t,e=%f\t\r\n",theta,e);
+ //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv);
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ 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);
+ //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
+ //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ //test1/31
+ float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
+ float alpha=0.8,beta=0.6;
+ float e_i,e_th;
+ float F_i,F_th,w_th;
+
+ LPF_th = alpha * LPF_thb + (1- alpha) * theta ;
+ e_th = LPF_th-LPF_thb;
+ w_th = e_th/16.4;
+ F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th;
+
+ LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ;
+ e_i = LPF_i-LPF_ib;
+ F_i = kp_i * (e_i -e_ib) + ki_i * e_i;
+
+
+ if(LPF_th < -3.0 && LPF_th > -20.0){
+ if(e_th > -0.5 && e_th < 0.5){
+ dc = (LPF_th/20.0);
+ M.setDutyCycle(dc);
+ }else{
+ dc = abs(F_th);
+ M.setDutyCycle(dc);
+ }
+ }else if(LPF_th < 20.0 && LPF_th >3.0){
+ if(e_th > -0.5 && e_th < 0.5){
+ dc = (LPF_th/20.0);
+ M.setDutyCycle(dc);
+ }else{
+ dc = -abs(F_th);
+ M.setDutyCycle(dc);
+ }
+ }else if(LPF_th <= 3.0 && LPF_th > 0.0){
+ dc = abs(F_th);
+ M.setDutyCycle(dc);
+
+ }else if(LPF_th < 0.0 && LPF_th >= -3.0){
+ dc = -abs(F_th);
+ M.setDutyCycle(dc);
+
+ }else if(LPF_th == 0){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+
+ e_thb = e_th;
+ LPF_thb = LPF_th;
+
+
+ Cv = Curr_vi;
+ //Vr_adc=V_adc.read();
+
+ //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv);
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ 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);
+ //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
+ //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
+*/
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+ /*
+ //test1/26
+ float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.1;
+ float alpha=0.8,beta=0.6;
+ float e_i,e_th;
+ float F_i,F_th;
+
+ LPF_th = alpha * LPF_thb + (1- alpha) * theta ;
+ e_th = LPF_th-LPF_thb;
+ F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th;
+
+ LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ;
+ e_i = LPF_i-LPF_ib;
+ F_i = kp_i * (e_i -e_ib) + ki_i * e_i;
+
+ if(LPF_th < 0 && LPF_th > -20.0){
+ //if(theta <0 && theta > -20.0){
+ //dc = (LPF_th/20.0) - F_th;
+ dc = F_th/2;
+ M.setDutyCycle(dc);
+ //dc = (theta/30.0)*1.0;
+
+ }else if(LPF_th < 20.0 && LPF_th >0){
+ //dc = (LPF_th/20.0) + F_th;
+ //dc = (theta/30.0)*1.0;
+ dc = F_th/2;
+ M.setDutyCycle(dc);
+ }else if(LPF_th == 0){
+ //}else if(LPF_th <= 1.0 && LPF_th >= -1.0){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+
+
+ //}
+ else{
+ if(theta <-4.0 && theta > -20.0){
+ dc = (LPF_/20.0) + F_th-0.1;
+ M.setDutyCycle(dc);
+ //dc = (theta/30.0)*1.0;
+
+ }else if(LPF_th < 30.0 && LPF_th >4.0){
+ dc = (LPF_th/20.0) + F_th+0.1;
+ //dc = (theta/30.0)*1.0;
+ M.setDutyCycle(dc);
+
+ }else if(LPF_th <= 0.5 && LPF_th >= -0.5){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+ }
+
+
+ e_thb = e_th;
+ LPF_thb = LPF_th;
+
+
+ Cv = Curr_vi;
+ //Vr_adc=V_adc.read();
+
+ //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv);
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ 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);
+ //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
+ //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ //ok
+ if(theta <-0.5 && theta > -30.0){
+ dc = (theta/30.0)*1.0;
+ M.setDutyCycle(dc);
+
+ }else if(theta < 30.0 && theta >0.5){
+ dc = (theta/30.0)*1.0;
+ M.setDutyCycle(dc);
+
+ }else if(theta <= 0.5 && theta >= -0.5){
+ dc = 0.0f;
+ M.setDutyCycle(dc);
+
+ }else{
+ dc = 0.5f;
+ M.setDutyCycle(dc);
+ }
+
+ Cv = Curr_vi;
+ //Vr_adc=V_adc.read();
+
+
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
+ printf("dc = %f, %f ,theta=%f\r\n",dc,Cv,theta);
+ //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
+ //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ t.start();
+ sectornow = M.getSector();
+ diffsector = sectornow - sectorbefore;
+ if(diffsector > 0 || abs(diffsector) == 5){
+ //t.stop();
+ rpm=60.0/t;
+ }else{
+ //t.stop();
+ rpm=0.0;
+ }
+ printf("dc=%f,%d,time=%d,rpm=%f,%d\r\n",dc,M.getSector(),t.read_ms(),rpm,diffsector);
+ //t.reset();
+
+
+
+ //if(diff<20){
+
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*//duty test1
+ char c = getchar();
+ if((c == 'w') && (dc < 0.9f)) {
+ dc += 0.1f;
+ M.setDutyCycle(dc);
+ }
+ if((c == 's') && (dc > -0.9f)) {
+ dc -= 0.1f;
+ M.setDutyCycle(dc);
+ }
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ //duty test2
+ /*
+ s = getsector();
+ diffsector = sectornow - sectorbefore;
+ count = count + diffsector;
+
+
+
+ sectorbefore = sectornow;
+
+ }
+
+ dc = 0.1;
+ M.setDutyCycle(dc);
+
+
+
+ rpm = 90*sector*60/t;
+
+
+ printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),diff);
+ */
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /*
+ if(abs(diff)<20 && abs(diff)>0){
+
+ dc = diff/20 * 0.5;
+
+ }else{
+
+ dc = 0.0;
+
+ }
+ */
+
+
+
+
+
+
+ //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),diff);
+ //printf("dc=%f,%d,time=%d,rpm=%f,%d\r\n",dc,M.getSector(),t.read_us(),rpm,diffsector);
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+ thetab = theta;
+ //sectorbefore = sectornow;
+ i++;
+ }
+
+}
\ No newline at end of file