Tatsuya Hiruma / Mbed 2 deprecated test2

gg

Dependencies:   mbed MPU6050 RateLimiter test Math

main2.cpp@8:7efca5258efb, 2020-03-06 (annotated)

Committer:
18fmr36
Date:
Fri Mar 06 05:58:45 2020 +0000
Revision:
8:7efca5258efb
gg

Who changed what in which revision?

UserRevisionLine numberNew contents of line
18fmr36 8:7efca5258efb 1 #include "mbed.h"
18fmr36 8:7efca5258efb 2
18fmr36 8:7efca5258efb 3 #include "MPU6050.h"
18fmr36 8:7efca5258efb 4 #include "x_nucleo_ihm07m1_targets.h"
18fmr36 8:7efca5258efb 5 #include "RateLimiter.h"
18fmr36 8:7efca5258efb 6 #include "SPN7Driver.h"
18fmr36 8:7efca5258efb 7
18fmr36 8:7efca5258efb 8
18fmr36 8:7efca5258efb 9 #include <math.h>
18fmr36 8:7efca5258efb 10 #include <stdio.h>
18fmr36 8:7efca5258efb 11 #include <time.h>
18fmr36 8:7efca5258efb 12
18fmr36 8:7efca5258efb 13
18fmr36 8:7efca5258efb 14 DigitalOut led1(LED1);
18fmr36 8:7efca5258efb 15 AnalogIn Curr_ui(PA_0);
18fmr36 8:7efca5258efb 16 AnalogIn Curr_vi(PC_1);
18fmr36 8:7efca5258efb 17 AnalogIn Curr_wi(PC_0);
18fmr36 8:7efca5258efb 18 AnalogIn V_adc(PC_2);//IHM08potentiometer
18fmr36 8:7efca5258efb 19 AnalogOut aout(PA_4);//IHM08DAC
18fmr36 8:7efca5258efb 20
18fmr36 8:7efca5258efb 21
18fmr36 8:7efca5258efb 22 MPU6050 mpu(D14,D15);
18fmr36 8:7efca5258efb 23 //Serial pc(USBTX, USBRX);
18fmr36 8:7efca5258efb 24 //Timer timer;
18fmr36 8:7efca5258efb 25
18fmr36 8:7efca5258efb 26 int diffsector,sectornow,sectorbefore = 0;
18fmr36 8:7efca5258efb 27 int c = 0,s;
18fmr36 8:7efca5258efb 28 float rpm;
18fmr36 8:7efca5258efb 29
18fmr36 8:7efca5258efb 30 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 31 // Instance of the motor driver
18fmr36 8:7efca5258efb 32 SPN7Driver M(
18fmr36 8:7efca5258efb 33 P_IN1, P_IN2, P_IN3, // Logic input pins
18fmr36 8:7efca5258efb 34 P_EN1, P_EN2, P_EN3, // Enable channel pins
18fmr36 8:7efca5258efb 35 P_HALL1, P_HALL2, P_HALL3, // Hall sensors pins
18fmr36 8:7efca5258efb 36 P_CURR1, P_CURR2, P_CURR3,
18fmr36 8:7efca5258efb 37 P_FAULT // Fault LED
18fmr36 8:7efca5258efb 38 );
18fmr36 8:7efca5258efb 39 // Pin to check temperature on the X-NUCLEO-IHM07M1 board
18fmr36 8:7efca5258efb 40 AnalogIn temperature(P_TEMP);
18fmr36 8:7efca5258efb 41
18fmr36 8:7efca5258efb 42 void checkTemperature()
18fmr36 8:7efca5258efb 43 {
18fmr36 8:7efca5258efb 44 if (temperature > 0.55f){
18fmr36 8:7efca5258efb 45 printf("Overheating... Turning off now\n\r");
18fmr36 8:7efca5258efb 46 M.setDutyCycle(0);
18fmr36 8:7efca5258efb 47 M.coast();
18fmr36 8:7efca5258efb 48 }
18fmr36 8:7efca5258efb 49 }
18fmr36 8:7efca5258efb 50 /*
18fmr36 8:7efca5258efb 51 void flip()
18fmr36 8:7efca5258efb 52 {
18fmr36 8:7efca5258efb 53
18fmr36 8:7efca5258efb 54 led1 =! led1;
18fmr36 8:7efca5258efb 55 sectornow = M.getSector();
18fmr36 8:7efca5258efb 56 diffsector = sectornow - sectorbefore;
18fmr36 8:7efca5258efb 57 sectorbefore = sectornow;
18fmr36 8:7efca5258efb 58 c = c + abs(diffsector);
18fmr36 8:7efca5258efb 59 rpm = 2/3*abs(diffsector);
18fmr36 8:7efca5258efb 60
18fmr36 8:7efca5258efb 61 //printf("%f\t\r\n",rpm);
18fmr36 8:7efca5258efb 62
18fmr36 8:7efca5258efb 63
18fmr36 8:7efca5258efb 64 }
18fmr36 8:7efca5258efb 65
18fmr36 8:7efca5258efb 66 */
18fmr36 8:7efca5258efb 67
18fmr36 8:7efca5258efb 68 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 69 int main(){
18fmr36 8:7efca5258efb 70 //float g[3];
18fmr36 8:7efca5258efb 71 //int miri=1000;
18fmr36 8:7efca5258efb 72 int accel[3],gyro[3];//accelを3つの配列で定義。
18fmr36 8:7efca5258efb 73 float diff,theta,thetab=0,difftheta,thetavel;
18fmr36 8:7efca5258efb 74 double a,b;
18fmr36 8:7efca5258efb 75 double PI=3.141592;
18fmr36 8:7efca5258efb 76 float K=0.1,T=0.005;
18fmr36 8:7efca5258efb 77 //float P_CURR1;
18fmr36 8:7efca5258efb 78 float dc = 0.0f;
18fmr36 8:7efca5258efb 79 int i=0, n=1000;
18fmr36 8:7efca5258efb 80 int diffsector,sectornow,sectorbefore = 0;
18fmr36 8:7efca5258efb 81 //float t=0.01;
18fmr36 8:7efca5258efb 82 float rpm ;
18fmr36 8:7efca5258efb 83 int pole=15;
18fmr36 8:7efca5258efb 84 Timer t ;
18fmr36 8:7efca5258efb 85 Ticker flipper;
18fmr36 8:7efca5258efb 86 //float CURR;
18fmr36 8:7efca5258efb 87 float Cw,Cv,Cu;
18fmr36 8:7efca5258efb 88 float Vr_adc;
18fmr36 8:7efca5258efb 89 float th,thb=0;
18fmr36 8:7efca5258efb 90 float dt =0.01;
18fmr36 8:7efca5258efb 91
18fmr36 8:7efca5258efb 92 float LPF_i,LPF_ib=0,LPF_th,LPF_thb=0;
18fmr36 8:7efca5258efb 93 float F_thb=0;
18fmr36 8:7efca5258efb 94 float ib=0,xgb=0;
18fmr36 8:7efca5258efb 95 float e_ib=0,e_thb=0,eib=0;
18fmr36 8:7efca5258efb 96 int gam;
18fmr36 8:7efca5258efb 97 //float kp=0.05,ki=0.01;
18fmr36 8:7efca5258efb 98
18fmr36 8:7efca5258efb 99 Ticker ticker;
18fmr36 8:7efca5258efb 100 ticker.attach(checkTemperature, 1); // Periodic overheating check
18fmr36 8:7efca5258efb 101 /*
18fmr36 8:7efca5258efb 102
18fmr36 8:7efca5258efb 103 */
18fmr36 8:7efca5258efb 104 //led1 = 1;
18fmr36 8:7efca5258efb 105 //flipper.attach_us(&flip,1000000.0);
18fmr36 8:7efca5258efb 106 //flipper.attach_us(&flip,10000.0);
18fmr36 8:7efca5258efb 107 while(1) {
18fmr36 8:7efca5258efb 108 //wait(1);
18fmr36 8:7efca5258efb 109 mpu.readGyroData(gyro);
18fmr36 8:7efca5258efb 110 float xg = gyro[0];
18fmr36 8:7efca5258efb 111 float yg = gyro[1];
18fmr36 8:7efca5258efb 112 float zg = gyro[2];
18fmr36 8:7efca5258efb 113
18fmr36 8:7efca5258efb 114 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
18fmr36 8:7efca5258efb 115 //int x = accel[0]-123;//x軸方向の加速度
18fmr36 8:7efca5258efb 116 float xa = accel[0];
18fmr36 8:7efca5258efb 117 //int y = accel[1]+60;//y軸方向の加速度
18fmr36 8:7efca5258efb 118 float ya = accel[1];
18fmr36 8:7efca5258efb 119 //int z = accel[2]+1110 ;//z軸方向の加速度
18fmr36 8:7efca5258efb 120 float za = accel[2];
18fmr36 8:7efca5258efb 121
18fmr36 8:7efca5258efb 122 a = ya;
18fmr36 8:7efca5258efb 123 b = za;
18fmr36 8:7efca5258efb 124 theta = atan2(a,b)*180/PI;
18fmr36 8:7efca5258efb 125
18fmr36 8:7efca5258efb 126 diff = theta - thetab;
18fmr36 8:7efca5258efb 127
18fmr36 8:7efca5258efb 128
18fmr36 8:7efca5258efb 129
18fmr36 8:7efca5258efb 130
18fmr36 8:7efca5258efb 131
18fmr36 8:7efca5258efb 132
18fmr36 8:7efca5258efb 133 //printf("%d,%f,%d,%d,%d,%f\r\n",c,theta,sectornow,sectorbefore,diffsector,rpm);
18fmr36 8:7efca5258efb 134 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 135 //theta1 = fabs(theta);
18fmr36 8:7efca5258efb 136 //myled = 1;
18fmr36 8:7efca5258efb 137 //wait(0.2);
18fmr36 8:7efca5258efb 138 //myled = 0;
18fmr36 8:7efca5258efb 139 //wait(0.2);
18fmr36 8:7efca5258efb 140
18fmr36 8:7efca5258efb 141 //mpu.getGyroRawZ();
18fmr36 8:7efca5258efb 142 //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
18fmr36 8:7efca5258efb 143 //writing current accelerometer and gyro position
18fmr36 8:7efca5258efb 144 //pc.printf("%d;%d;%d;%d;%d;%d\n",ax,ay,az,gx,gy,gz);
18fmr36 8:7efca5258efb 145
18fmr36 8:7efca5258efb 146 //printf("gyrz=%f\r\n",mpu.getAccelero());
18fmr36 8:7efca5258efb 147 //printf("gyrz=%f,accx=%f\r\n",mpu.getGyroRawZ(),mpu.getAcceleroRawX());
18fmr36 8:7efca5258efb 148 //printf("%4d,%4d,%f,%f,%f\r\n",x,y,a,b,theta);
18fmr36 8:7efca5258efb 149 //printf("%f\r\n",theta);
18fmr36 8:7efca5258efb 150 /*
18fmr36 8:7efca5258efb 151 timer.reset();
18fmr36 8:7efca5258efb 152 timer.start();
18fmr36 8:7efca5258efb 153 mpu.getGyro(g);
18fmr36 8:7efca5258efb 154 timer.stop();
18fmr36 8:7efca5258efb 155 float t=timer.read();
18fmr36 8:7efca5258efb 156 float time=t*miri;
18fmr36 8:7efca5258efb 157
18fmr36 8:7efca5258efb 158 pc.printf("time= %f ms\n\r",time);
18fmr36 8:7efca5258efb 159 */
18fmr36 8:7efca5258efb 160
18fmr36 8:7efca5258efb 161
18fmr36 8:7efca5258efb 162 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 163 /*
18fmr36 8:7efca5258efb 164 if(theta < -1 && theta > -30){
18fmr36 8:7efca5258efb 165 dc = (theta/30)*0.8+0.1;
18fmr36 8:7efca5258efb 166 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 167 }else if(theta < 30 && theta > 1){
18fmr36 8:7efca5258efb 168 dc = (theta/30)*0.8+0.1;
18fmr36 8:7efca5258efb 169 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 170 }else if(theta <= 1 && theta >= -1){
18fmr36 8:7efca5258efb 171 dc = 0.0f;
18fmr36 8:7efca5258efb 172 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 173 }else{
18fmr36 8:7efca5258efb 174 dc = 0.8f;
18fmr36 8:7efca5258efb 175 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 176 }
18fmr36 8:7efca5258efb 177 */
18fmr36 8:7efca5258efb 178 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 179 //2/5
18fmr36 8:7efca5258efb 180
18fmr36 8:7efca5258efb 181 float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
18fmr36 8:7efca5258efb 182 float alpha=0.8,beta=0.8;
18fmr36 8:7efca5258efb 183 float e_i,e_th;
18fmr36 8:7efca5258efb 184 float F_i,F_th;
18fmr36 8:7efca5258efb 185 float angle;
18fmr36 8:7efca5258efb 186
18fmr36 8:7efca5258efb 187 //LPF_i = alpha * LPF_ib + (1 - alpha) * Curr_vi;
18fmr36 8:7efca5258efb 188 //e_i = LPF_i - ib;
18fmr36 8:7efca5258efb 189 //ui = Fi*ei;
18fmr36 8:7efca5258efb 190 //LPF_th = beta * LPF_thb + (1- beta) * theta ;
18fmr36 8:7efca5258efb 191 th = 0.05 * (thb + yg * dt) + 0.95 * theta;
18fmr36 8:7efca5258efb 192
18fmr36 8:7efca5258efb 193 sectornow = M.getSector();
18fmr36 8:7efca5258efb 194 /*
18fmr36 8:7efca5258efb 195 diffsector = sectornow - sectorbefore;
18fmr36 8:7efca5258efb 196 if(sector=)
18fmr36 8:7efca5258efb 197
18fmr36 8:7efca5258efb 198
18fmr36 8:7efca5258efb 199 if(diffsector==5 || diffsector==-5){
18fmr36 8:7efca5258efb 200 diffsector=1;
18fmr36 8:7efca5258efb 201 }else if(diffsector==4 || diffsector==-4){
18fmr36 8:7efca5258efb 202 diffsector=2;
18fmr36 8:7efca5258efb 203 }
18fmr36 8:7efca5258efb 204
18fmr36 8:7efca5258efb 205
18fmr36 8:7efca5258efb 206 sectorbefore = sectornow;
18fmr36 8:7efca5258efb 207
18fmr36 8:7efca5258efb 208 angle = diffsector*4;
18fmr36 8:7efca5258efb 209 c = c + abs(diffsector);
18fmr36 8:7efca5258efb 210 rpm = 2/3*abs(diffsector);
18fmr36 8:7efca5258efb 211 */
18fmr36 8:7efca5258efb 212
18fmr36 8:7efca5258efb 213
18fmr36 8:7efca5258efb 214
18fmr36 8:7efca5258efb 215
18fmr36 8:7efca5258efb 216
18fmr36 8:7efca5258efb 217 //LPF_th = LPF_th;
18fmr36 8:7efca5258efb 218 //e_th = LPF_th-LPF_thb;
18fmr36 8:7efca5258efb 219 e_th = th-thb;
18fmr36 8:7efca5258efb 220
18fmr36 8:7efca5258efb 221 if(th <= -1.0 && th> -15.0){
18fmr36 8:7efca5258efb 222 dc=+abs((th/70.0)+4.0/140.0);
18fmr36 8:7efca5258efb 223 //dc=abs(th/15)+0.1;
18fmr36 8:7efca5258efb 224 }else if(th < 15.0 && th >= 1.0){
18fmr36 8:7efca5258efb 225 dc=-abs((th/70.0)+4.0/140.0);
18fmr36 8:7efca5258efb 226 //dc=-abs(th/15)-0.1;
18fmr36 8:7efca5258efb 227 }else if(th>-1 && th<1){
18fmr36 8:7efca5258efb 228 dc=0.0f;
18fmr36 8:7efca5258efb 229 //M.coast();
18fmr36 8:7efca5258efb 230 }else if(th < -15.0 ){
18fmr36 8:7efca5258efb 231 dc=0.3f;
18fmr36 8:7efca5258efb 232 }else if(th > 15.0 ){
18fmr36 8:7efca5258efb 233 dc=-0.3f;
18fmr36 8:7efca5258efb 234 }else{
18fmr36 8:7efca5258efb 235 dc=0.0f;
18fmr36 8:7efca5258efb 236 }
18fmr36 8:7efca5258efb 237
18fmr36 8:7efca5258efb 238 //dc = (th/15.0);
18fmr36 8:7efca5258efb 239 //dc = 0.3f;
18fmr36 8:7efca5258efb 240 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 241
18fmr36 8:7efca5258efb 242 Cv = Curr_vi;
18fmr36 8:7efca5258efb 243
18fmr36 8:7efca5258efb 244 //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);
18fmr36 8:7efca5258efb 245 //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);
18fmr36 8:7efca5258efb 246 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);
18fmr36 8:7efca5258efb 247 sectorbefore = sectornow;
18fmr36 8:7efca5258efb 248 thb=th;
18fmr36 8:7efca5258efb 249 LPF_thb=LPF_th;
18fmr36 8:7efca5258efb 250
18fmr36 8:7efca5258efb 251 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 252 /*
18fmr36 8:7efca5258efb 253 if(theta <= -4.0 && theta > -20.0){
18fmr36 8:7efca5258efb 254 dc = (theta/20.0)*0.5;
18fmr36 8:7efca5258efb 255 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 256
18fmr36 8:7efca5258efb 257 }else if(theta < 20.0 && theta >= 4.0){
18fmr36 8:7efca5258efb 258 dc = (theta/20.0)*0.5;
18fmr36 8:7efca5258efb 259 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 260
18fmr36 8:7efca5258efb 261 }else if(theta == 0.0){
18fmr36 8:7efca5258efb 262 dc = 0.1f;
18fmr36 8:7efca5258efb 263 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 264
18fmr36 8:7efca5258efb 265 }else if(theta > 0.05 && theta < 4.0){
18fmr36 8:7efca5258efb 266 dc = K*(1-exp(-theta1/T));
18fmr36 8:7efca5258efb 267 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 268
18fmr36 8:7efca5258efb 269 }else if(theta > -4.0 && theta < -0.05){
18fmr36 8:7efca5258efb 270 dc = -K*(1-exp(-theta1/T));
18fmr36 8:7efca5258efb 271 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 272
18fmr36 8:7efca5258efb 273 }else{
18fmr36 8:7efca5258efb 274 dc = 0.5f;
18fmr36 8:7efca5258efb 275 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 276 }
18fmr36 8:7efca5258efb 277 printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
18fmr36 8:7efca5258efb 278 */
18fmr36 8:7efca5258efb 279 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 280 /*
18fmr36 8:7efca5258efb 281 difftheta = theta1-theta ;
18fmr36 8:7efca5258efb 282 thetavel = difftheta/0.01;
18fmr36 8:7efca5258efb 283
18fmr36 8:7efca5258efb 284 //dc = dc + kp*(theta - theta1)+ki*theta;
18fmr36 8:7efca5258efb 285
18fmr36 8:7efca5258efb 286
18fmr36 8:7efca5258efb 287
18fmr36 8:7efca5258efb 288
18fmr36 8:7efca5258efb 289 if(theta <-0.5 && theta > -30.0){
18fmr36 8:7efca5258efb 290 //dc = (theta/30.0)*1.0;
18fmr36 8:7efca5258efb 291 dc = kp*(theta - theta1)+ki*theta;
18fmr36 8:7efca5258efb 292 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 293 //if(thetavel){}
18fmr36 8:7efca5258efb 294
18fmr36 8:7efca5258efb 295
18fmr36 8:7efca5258efb 296
18fmr36 8:7efca5258efb 297
18fmr36 8:7efca5258efb 298 }else if(theta < 30.0 && theta >0.5){
18fmr36 8:7efca5258efb 299 //dc = (theta/30.0)*1.0;
18fmr36 8:7efca5258efb 300 dc = kp*(theta - theta1)+ki*theta;
18fmr36 8:7efca5258efb 301 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 302
18fmr36 8:7efca5258efb 303 }else if(theta <= 0.5 && theta >= -0.5){
18fmr36 8:7efca5258efb 304 dc = 0.0f;
18fmr36 8:7efca5258efb 305 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 306
18fmr36 8:7efca5258efb 307 }else{
18fmr36 8:7efca5258efb 308 dc = 0.5f;
18fmr36 8:7efca5258efb 309 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 310 }
18fmr36 8:7efca5258efb 311
18fmr36 8:7efca5258efb 312 Cw = Curr_wi.read();
18fmr36 8:7efca5258efb 313 Vr_adc=V_adc.read();
18fmr36 8:7efca5258efb 314
18fmr36 8:7efca5258efb 315
18fmr36 8:7efca5258efb 316 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
18fmr36 8:7efca5258efb 317 //printf("dc = %f, %f ,theta=%f\r\n",dc,Cw,theta);
18fmr36 8:7efca5258efb 318 //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
18fmr36 8:7efca5258efb 319 printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
18fmr36 8:7efca5258efb 320 */
18fmr36 8:7efca5258efb 321 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 322 //LPF
18fmr36 8:7efca5258efb 323 /*
18fmr36 8:7efca5258efb 324 float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
18fmr36 8:7efca5258efb 325 float alpha=0.8,beta=0.8;
18fmr36 8:7efca5258efb 326 float e_i,e_th;
18fmr36 8:7efca5258efb 327 float F_i,F_th;
18fmr36 8:7efca5258efb 328
18fmr36 8:7efca5258efb 329 LPF_i = alpha * LPF_ib + (1 - alpha) * Curr_vi;
18fmr36 8:7efca5258efb 330 e_i = LPF_i - ib;
18fmr36 8:7efca5258efb 331 //ui = Fi*ei;
18fmr36 8:7efca5258efb 332
18fmr36 8:7efca5258efb 333 LPF_th = beta * LPF_thb + (1- beta) * xg/16.4 ;
18fmr36 8:7efca5258efb 334 LPF_th = LPF_th;
18fmr36 8:7efca5258efb 335 e_th = LPF_th-LPF_thb;
18fmr36 8:7efca5258efb 336
18fmr36 8:7efca5258efb 337 if(theta > -2.0 && theta < 2.0){
18fmr36 8:7efca5258efb 338 gam = 0;
18fmr36 8:7efca5258efb 339 }else{
18fmr36 8:7efca5258efb 340 gam = 1;
18fmr36 8:7efca5258efb 341 }
18fmr36 8:7efca5258efb 342
18fmr36 8:7efca5258efb 343 //if()
18fmr36 8:7efca5258efb 344
18fmr36 8:7efca5258efb 345 F_i = kp_i * (e_i - e_ib) + ki_i * e_i;
18fmr36 8:7efca5258efb 346 F_th = kp_th * (e_th -e_thb) + ki_th * e_th;
18fmr36 8:7efca5258efb 347
18fmr36 8:7efca5258efb 348
18fmr36 8:7efca5258efb 349 dc = gam * F_th *(LPF_th - xgb) + (1 - gam) * F_i*(LPF_i - ib);
18fmr36 8:7efca5258efb 350 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 351
18fmr36 8:7efca5258efb 352
18fmr36 8:7efca5258efb 353 //printf("xg=%f\t,%d\r\n",xg,gam);
18fmr36 8:7efca5258efb 354 //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);
18fmr36 8:7efca5258efb 355 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);
18fmr36 8:7efca5258efb 356
18fmr36 8:7efca5258efb 357 ib = Curr_vi;
18fmr36 8:7efca5258efb 358 LPF_ib = LPF_i;
18fmr36 8:7efca5258efb 359 LPF_thb = LPF_th;
18fmr36 8:7efca5258efb 360 xgb = xg;
18fmr36 8:7efca5258efb 361 */
18fmr36 8:7efca5258efb 362 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 363
18fmr36 8:7efca5258efb 364
18fmr36 8:7efca5258efb 365
18fmr36 8:7efca5258efb 366
18fmr36 8:7efca5258efb 367
18fmr36 8:7efca5258efb 368
18fmr36 8:7efca5258efb 369
18fmr36 8:7efca5258efb 370
18fmr36 8:7efca5258efb 371
18fmr36 8:7efca5258efb 372
18fmr36 8:7efca5258efb 373
18fmr36 8:7efca5258efb 374
18fmr36 8:7efca5258efb 375
18fmr36 8:7efca5258efb 376 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 377 /*
18fmr36 8:7efca5258efb 378 //test2/1
18fmr36 8:7efca5258efb 379 float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
18fmr36 8:7efca5258efb 380 float alpha=0.8,beta=0.6;
18fmr36 8:7efca5258efb 381 float e_i,e_th,e,ei,u;
18fmr36 8:7efca5258efb 382 float F_i,F_th,w_th;
18fmr36 8:7efca5258efb 383
18fmr36 8:7efca5258efb 384 th = 0.05 * (thb + yg * dt) + 0.95 * theta;
18fmr36 8:7efca5258efb 385 //LPF_th = alpha * LPF_thb + (1- alpha) * theta ;
18fmr36 8:7efca5258efb 386
18fmr36 8:7efca5258efb 387 e = 0 - LPF_th;
18fmr36 8:7efca5258efb 388 ei = eib + e * 0.01;
18fmr36 8:7efca5258efb 389 u = kp_th * e + ki_th * ei;
18fmr36 8:7efca5258efb 390
18fmr36 8:7efca5258efb 391 e_th = LPF_th-LPF_thb;
18fmr36 8:7efca5258efb 392 w_th = e_th/16.4;
18fmr36 8:7efca5258efb 393 F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th;
18fmr36 8:7efca5258efb 394
18fmr36 8:7efca5258efb 395 LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ;
18fmr36 8:7efca5258efb 396 e_i = LPF_i-LPF_ib;
18fmr36 8:7efca5258efb 397 F_i = kp_i * (e_i -e_ib) + ki_i * e_i;
18fmr36 8:7efca5258efb 398 if(e_th < 0.05 && e_th > -0.05){
18fmr36 8:7efca5258efb 399 e_th =0;
18fmr36 8:7efca5258efb 400 }
18fmr36 8:7efca5258efb 401
18fmr36 8:7efca5258efb 402 if(th < 0.0 && th > -20.0){
18fmr36 8:7efca5258efb 403 if(e_th > -1.5 && e_th < 1.5){
18fmr36 8:7efca5258efb 404 if(e_th >0){
18fmr36 8:7efca5258efb 405 dc=th/20;
18fmr36 8:7efca5258efb 406 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 407 }else{
18fmr36 8:7efca5258efb 408 dc=-th/20;
18fmr36 8:7efca5258efb 409 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 410 }
18fmr36 8:7efca5258efb 411 }else{
18fmr36 8:7efca5258efb 412 dc = (e_th/6.0);
18fmr36 8:7efca5258efb 413 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 414 }
18fmr36 8:7efca5258efb 415 }else if(th < 20.0 && th >0.0){
18fmr36 8:7efca5258efb 416 if(e_th > -1.5 && e_th < 1.5){
18fmr36 8:7efca5258efb 417 if(e_th >0){
18fmr36 8:7efca5258efb 418 dc=th/20;
18fmr36 8:7efca5258efb 419 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 420 }else{
18fmr36 8:7efca5258efb 421 dc=-th/20;
18fmr36 8:7efca5258efb 422 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 423 }
18fmr36 8:7efca5258efb 424 }else{
18fmr36 8:7efca5258efb 425 dc = (e_th/6.0);
18fmr36 8:7efca5258efb 426 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 427 }
18fmr36 8:7efca5258efb 428 }else {
18fmr36 8:7efca5258efb 429 dc = 0.5f;
18fmr36 8:7efca5258efb 430 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 431 }
18fmr36 8:7efca5258efb 432
18fmr36 8:7efca5258efb 433 eib = ei;
18fmr36 8:7efca5258efb 434 e_thb = e_th;
18fmr36 8:7efca5258efb 435 LPF_thb = LPF_th;
18fmr36 8:7efca5258efb 436 thb=th;
18fmr36 8:7efca5258efb 437
18fmr36 8:7efca5258efb 438 Cv = Curr_vi;
18fmr36 8:7efca5258efb 439 //Vr_adc=V_adc.read();
18fmr36 8:7efca5258efb 440 //printf("theta=%f\t,e=%f\t\r\n",theta,e);
18fmr36 8:7efca5258efb 441 //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv);
18fmr36 8:7efca5258efb 442 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
18fmr36 8:7efca5258efb 443 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);
18fmr36 8:7efca5258efb 444 //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
18fmr36 8:7efca5258efb 445 //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
18fmr36 8:7efca5258efb 446 */
18fmr36 8:7efca5258efb 447 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 448 /*
18fmr36 8:7efca5258efb 449 //test1/31
18fmr36 8:7efca5258efb 450 float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.01;
18fmr36 8:7efca5258efb 451 float alpha=0.8,beta=0.6;
18fmr36 8:7efca5258efb 452 float e_i,e_th;
18fmr36 8:7efca5258efb 453 float F_i,F_th,w_th;
18fmr36 8:7efca5258efb 454
18fmr36 8:7efca5258efb 455 LPF_th = alpha * LPF_thb + (1- alpha) * theta ;
18fmr36 8:7efca5258efb 456 e_th = LPF_th-LPF_thb;
18fmr36 8:7efca5258efb 457 w_th = e_th/16.4;
18fmr36 8:7efca5258efb 458 F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th;
18fmr36 8:7efca5258efb 459
18fmr36 8:7efca5258efb 460 LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ;
18fmr36 8:7efca5258efb 461 e_i = LPF_i-LPF_ib;
18fmr36 8:7efca5258efb 462 F_i = kp_i * (e_i -e_ib) + ki_i * e_i;
18fmr36 8:7efca5258efb 463
18fmr36 8:7efca5258efb 464
18fmr36 8:7efca5258efb 465 if(LPF_th < -3.0 && LPF_th > -20.0){
18fmr36 8:7efca5258efb 466 if(e_th > -0.5 && e_th < 0.5){
18fmr36 8:7efca5258efb 467 dc = (LPF_th/20.0);
18fmr36 8:7efca5258efb 468 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 469 }else{
18fmr36 8:7efca5258efb 470 dc = abs(F_th);
18fmr36 8:7efca5258efb 471 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 472 }
18fmr36 8:7efca5258efb 473 }else if(LPF_th < 20.0 && LPF_th >3.0){
18fmr36 8:7efca5258efb 474 if(e_th > -0.5 && e_th < 0.5){
18fmr36 8:7efca5258efb 475 dc = (LPF_th/20.0);
18fmr36 8:7efca5258efb 476 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 477 }else{
18fmr36 8:7efca5258efb 478 dc = -abs(F_th);
18fmr36 8:7efca5258efb 479 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 480 }
18fmr36 8:7efca5258efb 481 }else if(LPF_th <= 3.0 && LPF_th > 0.0){
18fmr36 8:7efca5258efb 482 dc = abs(F_th);
18fmr36 8:7efca5258efb 483 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 484
18fmr36 8:7efca5258efb 485 }else if(LPF_th < 0.0 && LPF_th >= -3.0){
18fmr36 8:7efca5258efb 486 dc = -abs(F_th);
18fmr36 8:7efca5258efb 487 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 488
18fmr36 8:7efca5258efb 489 }else if(LPF_th == 0){
18fmr36 8:7efca5258efb 490 dc = 0.0f;
18fmr36 8:7efca5258efb 491 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 492
18fmr36 8:7efca5258efb 493 }else{
18fmr36 8:7efca5258efb 494 dc = 0.5f;
18fmr36 8:7efca5258efb 495 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 496 }
18fmr36 8:7efca5258efb 497
18fmr36 8:7efca5258efb 498 e_thb = e_th;
18fmr36 8:7efca5258efb 499 LPF_thb = LPF_th;
18fmr36 8:7efca5258efb 500
18fmr36 8:7efca5258efb 501
18fmr36 8:7efca5258efb 502 Cv = Curr_vi;
18fmr36 8:7efca5258efb 503 //Vr_adc=V_adc.read();
18fmr36 8:7efca5258efb 504
18fmr36 8:7efca5258efb 505 //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv);
18fmr36 8:7efca5258efb 506 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
18fmr36 8:7efca5258efb 507 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);
18fmr36 8:7efca5258efb 508 //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
18fmr36 8:7efca5258efb 509 //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
18fmr36 8:7efca5258efb 510 */
18fmr36 8:7efca5258efb 511 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 512
18fmr36 8:7efca5258efb 513 /*
18fmr36 8:7efca5258efb 514 //test1/26
18fmr36 8:7efca5258efb 515 float kp_i=0.8,kp_th=0.8,ki_i=0.01,ki_th=0.1;
18fmr36 8:7efca5258efb 516 float alpha=0.8,beta=0.6;
18fmr36 8:7efca5258efb 517 float e_i,e_th;
18fmr36 8:7efca5258efb 518 float F_i,F_th;
18fmr36 8:7efca5258efb 519
18fmr36 8:7efca5258efb 520 LPF_th = alpha * LPF_thb + (1- alpha) * theta ;
18fmr36 8:7efca5258efb 521 e_th = LPF_th-LPF_thb;
18fmr36 8:7efca5258efb 522 F_th = F_thb + kp_th * (e_th -e_thb) + ki_th * e_th;
18fmr36 8:7efca5258efb 523
18fmr36 8:7efca5258efb 524 LPF_i = beta * LPF_ib + (1- beta) * Curr_vi ;
18fmr36 8:7efca5258efb 525 e_i = LPF_i-LPF_ib;
18fmr36 8:7efca5258efb 526 F_i = kp_i * (e_i -e_ib) + ki_i * e_i;
18fmr36 8:7efca5258efb 527
18fmr36 8:7efca5258efb 528 if(LPF_th < 0 && LPF_th > -20.0){
18fmr36 8:7efca5258efb 529 //if(theta <0 && theta > -20.0){
18fmr36 8:7efca5258efb 530 //dc = (LPF_th/20.0) - F_th;
18fmr36 8:7efca5258efb 531 dc = F_th/2;
18fmr36 8:7efca5258efb 532 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 533 //dc = (theta/30.0)*1.0;
18fmr36 8:7efca5258efb 534
18fmr36 8:7efca5258efb 535 }else if(LPF_th < 20.0 && LPF_th >0){
18fmr36 8:7efca5258efb 536 //dc = (LPF_th/20.0) + F_th;
18fmr36 8:7efca5258efb 537 //dc = (theta/30.0)*1.0;
18fmr36 8:7efca5258efb 538 dc = F_th/2;
18fmr36 8:7efca5258efb 539 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 540 }else if(LPF_th == 0){
18fmr36 8:7efca5258efb 541 //}else if(LPF_th <= 1.0 && LPF_th >= -1.0){
18fmr36 8:7efca5258efb 542 dc = 0.0f;
18fmr36 8:7efca5258efb 543 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 544
18fmr36 8:7efca5258efb 545 }else{
18fmr36 8:7efca5258efb 546 dc = 0.5f;
18fmr36 8:7efca5258efb 547 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 548 }
18fmr36 8:7efca5258efb 549
18fmr36 8:7efca5258efb 550
18fmr36 8:7efca5258efb 551 //}
18fmr36 8:7efca5258efb 552 else{
18fmr36 8:7efca5258efb 553 if(theta <-4.0 && theta > -20.0){
18fmr36 8:7efca5258efb 554 dc = (LPF_/20.0) + F_th-0.1;
18fmr36 8:7efca5258efb 555 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 556 //dc = (theta/30.0)*1.0;
18fmr36 8:7efca5258efb 557
18fmr36 8:7efca5258efb 558 }else if(LPF_th < 30.0 && LPF_th >4.0){
18fmr36 8:7efca5258efb 559 dc = (LPF_th/20.0) + F_th+0.1;
18fmr36 8:7efca5258efb 560 //dc = (theta/30.0)*1.0;
18fmr36 8:7efca5258efb 561 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 562
18fmr36 8:7efca5258efb 563 }else if(LPF_th <= 0.5 && LPF_th >= -0.5){
18fmr36 8:7efca5258efb 564 dc = 0.0f;
18fmr36 8:7efca5258efb 565 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 566
18fmr36 8:7efca5258efb 567 }else{
18fmr36 8:7efca5258efb 568 dc = 0.5f;
18fmr36 8:7efca5258efb 569 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 570 }
18fmr36 8:7efca5258efb 571 }
18fmr36 8:7efca5258efb 572
18fmr36 8:7efca5258efb 573
18fmr36 8:7efca5258efb 574 e_thb = e_th;
18fmr36 8:7efca5258efb 575 LPF_thb = LPF_th;
18fmr36 8:7efca5258efb 576
18fmr36 8:7efca5258efb 577
18fmr36 8:7efca5258efb 578 Cv = Curr_vi;
18fmr36 8:7efca5258efb 579 //Vr_adc=V_adc.read();
18fmr36 8:7efca5258efb 580
18fmr36 8:7efca5258efb 581 //printf("theta=%f\t,dc=%f\t,Cv=%f\t\r\n",theta,dc,Cv);
18fmr36 8:7efca5258efb 582 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
18fmr36 8:7efca5258efb 583 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);
18fmr36 8:7efca5258efb 584 //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
18fmr36 8:7efca5258efb 585 //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
18fmr36 8:7efca5258efb 586 */
18fmr36 8:7efca5258efb 587 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 588 /*
18fmr36 8:7efca5258efb 589 //ok
18fmr36 8:7efca5258efb 590 if(theta <-0.5 && theta > -30.0){
18fmr36 8:7efca5258efb 591 dc = (theta/30.0)*1.0;
18fmr36 8:7efca5258efb 592 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 593
18fmr36 8:7efca5258efb 594 }else if(theta < 30.0 && theta >0.5){
18fmr36 8:7efca5258efb 595 dc = (theta/30.0)*1.0;
18fmr36 8:7efca5258efb 596 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 597
18fmr36 8:7efca5258efb 598 }else if(theta <= 0.5 && theta >= -0.5){
18fmr36 8:7efca5258efb 599 dc = 0.0f;
18fmr36 8:7efca5258efb 600 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 601
18fmr36 8:7efca5258efb 602 }else{
18fmr36 8:7efca5258efb 603 dc = 0.5f;
18fmr36 8:7efca5258efb 604 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 605 }
18fmr36 8:7efca5258efb 606
18fmr36 8:7efca5258efb 607 Cv = Curr_vi;
18fmr36 8:7efca5258efb 608 //Vr_adc=V_adc.read();
18fmr36 8:7efca5258efb 609
18fmr36 8:7efca5258efb 610
18fmr36 8:7efca5258efb 611 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),theta);
18fmr36 8:7efca5258efb 612 printf("dc = %f, %f ,theta=%f\r\n",dc,Cv,theta);
18fmr36 8:7efca5258efb 613 //printf("dc = %f, %f , %f ,theta=%f\r\n",dc,V_adc,aout,theta);
18fmr36 8:7efca5258efb 614 //printf("dc = %f,theta= %f,thetavel = %f\r\n",dc,theta,thetavel);
18fmr36 8:7efca5258efb 615 */
18fmr36 8:7efca5258efb 616 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 617 /*
18fmr36 8:7efca5258efb 618 t.start();
18fmr36 8:7efca5258efb 619 sectornow = M.getSector();
18fmr36 8:7efca5258efb 620 diffsector = sectornow - sectorbefore;
18fmr36 8:7efca5258efb 621 if(diffsector > 0 || abs(diffsector) == 5){
18fmr36 8:7efca5258efb 622 //t.stop();
18fmr36 8:7efca5258efb 623 rpm=60.0/t;
18fmr36 8:7efca5258efb 624 }else{
18fmr36 8:7efca5258efb 625 //t.stop();
18fmr36 8:7efca5258efb 626 rpm=0.0;
18fmr36 8:7efca5258efb 627 }
18fmr36 8:7efca5258efb 628 printf("dc=%f,%d,time=%d,rpm=%f,%d\r\n",dc,M.getSector(),t.read_ms(),rpm,diffsector);
18fmr36 8:7efca5258efb 629 //t.reset();
18fmr36 8:7efca5258efb 630
18fmr36 8:7efca5258efb 631
18fmr36 8:7efca5258efb 632
18fmr36 8:7efca5258efb 633 //if(diff<20){
18fmr36 8:7efca5258efb 634
18fmr36 8:7efca5258efb 635 */
18fmr36 8:7efca5258efb 636 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 637
18fmr36 8:7efca5258efb 638
18fmr36 8:7efca5258efb 639
18fmr36 8:7efca5258efb 640
18fmr36 8:7efca5258efb 641
18fmr36 8:7efca5258efb 642
18fmr36 8:7efca5258efb 643
18fmr36 8:7efca5258efb 644
18fmr36 8:7efca5258efb 645
18fmr36 8:7efca5258efb 646
18fmr36 8:7efca5258efb 647
18fmr36 8:7efca5258efb 648
18fmr36 8:7efca5258efb 649
18fmr36 8:7efca5258efb 650
18fmr36 8:7efca5258efb 651
18fmr36 8:7efca5258efb 652 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 653 /*//duty test1
18fmr36 8:7efca5258efb 654 char c = getchar();
18fmr36 8:7efca5258efb 655 if((c == 'w') && (dc < 0.9f)) {
18fmr36 8:7efca5258efb 656 dc += 0.1f;
18fmr36 8:7efca5258efb 657 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 658 }
18fmr36 8:7efca5258efb 659 if((c == 's') && (dc > -0.9f)) {
18fmr36 8:7efca5258efb 660 dc -= 0.1f;
18fmr36 8:7efca5258efb 661 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 662 }
18fmr36 8:7efca5258efb 663 */
18fmr36 8:7efca5258efb 664 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 665 //duty test2
18fmr36 8:7efca5258efb 666 /*
18fmr36 8:7efca5258efb 667 s = getsector();
18fmr36 8:7efca5258efb 668 diffsector = sectornow - sectorbefore;
18fmr36 8:7efca5258efb 669 count = count + diffsector;
18fmr36 8:7efca5258efb 670
18fmr36 8:7efca5258efb 671
18fmr36 8:7efca5258efb 672
18fmr36 8:7efca5258efb 673 sectorbefore = sectornow;
18fmr36 8:7efca5258efb 674
18fmr36 8:7efca5258efb 675 }
18fmr36 8:7efca5258efb 676
18fmr36 8:7efca5258efb 677 dc = 0.1;
18fmr36 8:7efca5258efb 678 M.setDutyCycle(dc);
18fmr36 8:7efca5258efb 679
18fmr36 8:7efca5258efb 680
18fmr36 8:7efca5258efb 681
18fmr36 8:7efca5258efb 682 rpm = 90*sector*60/t;
18fmr36 8:7efca5258efb 683
18fmr36 8:7efca5258efb 684
18fmr36 8:7efca5258efb 685 printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),diff);
18fmr36 8:7efca5258efb 686 */
18fmr36 8:7efca5258efb 687 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 688 /*
18fmr36 8:7efca5258efb 689 if(abs(diff)<20 && abs(diff)>0){
18fmr36 8:7efca5258efb 690
18fmr36 8:7efca5258efb 691 dc = diff/20 * 0.5;
18fmr36 8:7efca5258efb 692
18fmr36 8:7efca5258efb 693 }else{
18fmr36 8:7efca5258efb 694
18fmr36 8:7efca5258efb 695 dc = 0.0;
18fmr36 8:7efca5258efb 696
18fmr36 8:7efca5258efb 697 }
18fmr36 8:7efca5258efb 698 */
18fmr36 8:7efca5258efb 699
18fmr36 8:7efca5258efb 700
18fmr36 8:7efca5258efb 701
18fmr36 8:7efca5258efb 702
18fmr36 8:7efca5258efb 703
18fmr36 8:7efca5258efb 704
18fmr36 8:7efca5258efb 705 //printf("dc=%f,%d,theta=%f\r\n",dc,M.getSector(),diff);
18fmr36 8:7efca5258efb 706 //printf("dc=%f,%d,time=%d,rpm=%f,%d\r\n",dc,M.getSector(),t.read_us(),rpm,diffsector);
18fmr36 8:7efca5258efb 707 ////////////////////////////////////////////////////////////////////////////////////////////////////////////
18fmr36 8:7efca5258efb 708
18fmr36 8:7efca5258efb 709
18fmr36 8:7efca5258efb 710
18fmr36 8:7efca5258efb 711
18fmr36 8:7efca5258efb 712
18fmr36 8:7efca5258efb 713 thetab = theta;
18fmr36 8:7efca5258efb 714 //sectorbefore = sectornow;
18fmr36 8:7efca5258efb 715 i++;
18fmr36 8:7efca5258efb 716 }
18fmr36 8:7efca5258efb 717
18fmr36 8:7efca5258efb 718 }