solaESKF_EIGEN
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
Diff: main.cpp
- Revision:
- 24:6ab61527855d
- Parent:
- 23:4a490fa8bf4a
- Child:
- 25:5aca9b224226
--- a/main.cpp Mon Feb 15 06:57:00 2021 +0000 +++ b/main.cpp Mon Feb 15 07:14:44 2021 +0000 @@ -68,6 +68,8 @@ double acc_x,acc_y,acc_z; double gyro_x,gyro_y,gyro_z; int out1, out2; +int scaler_servo[3]; +int servo_out[3]; const double pitch_align = 0.0; const double roll_align = -0.0; @@ -91,7 +93,7 @@ { //LoopTicker sdcard_loop; LoopTicker PIDtick; - //PIDtick.attach(test_control,RATE); + //PIDtick.attach(test_control,PID_dt); //sdcard_loop.attach(pushto_sdcard,0.01); pc.baud(115200); //sd.baud(115200); @@ -115,7 +117,8 @@ MadgwickFilter.begin(100); //サンプリング周波数Hza // のセットアップ test_control.setInputLimits(-60.0,60.0); // 60°が限界 - test_control.setOutputLimits(-1,1); + test_control.setOutputLimits(1200, 1800); + test_control.setBias((1200+1800)/2); // out range の真ん中に設定する(out rangeの中央が安定点のため) t.start(); float LP_rc = 0.65f; while(1) { @@ -149,13 +152,13 @@ roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180; //float LP_rc3 = 0.15f; - //rc1 = LP_rc*float(map(roll * 180.0 / pi,-60,60,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1; + roll_ac = LP_rc*float(map(roll * 180.0 / pi,-60,60,-1000,1000))/1000.0f+(1.0f-LP_rc)*roll_ac; //rc1 = LP_rc * float(map(ch1,368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * rc1; // mapped input //rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2; int pwmMax = 1800; int pwmMin = 1200; - out1 = map((int)(rc[1]*1000.0f),-1000,1000,pwmMin,pwmMax); + out1 = map((int)(de _left*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用 if(out1<pwmMin) { out1 = pwmMin; }; @@ -179,8 +182,28 @@ MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza pc.printf("time%f \r\n", (tend-tstart)); test_control.setProcessValue(roll * 180.0 / pi); // 入力はこどほう - //out1 = test_control.compute(); - pc.printf("out1:%d ",out1); - servoRight.pulsewidth_us(out1); + scaled_servo[0] = test_control.compute(); // -1 ~ 1 + servo_out[0] = map((int)(scaler_servo[0]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用 + for (int i = 0; i < 3 ; i ++){ + servo_out[i] = max(servo_out[i],pwemMin); + servo_out[i] = min(servo_out[i], pwmMax); + } + /* + if(out1<pwmMin) { + out1 = pwmMin; + }; + if(out1>pwmMax) { + out1 = pwmMax; + }; + out2 = map((int)(rc[2]*1000.0f),-1000,1000,pwmMin,pwmMax); + if(out2<pwmMin) { + out2 = pwmMin; + }; + if(out2>pwmMax) { + out2 = pwmMax; + }; + */ + pc.printf("out1:%d ",servo_out[i]); + servoRight.pulsewidth_us(servo_out[i]); } } \ No newline at end of file