![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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