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:
- 22:da9893b71f24
- Parent:
- 21:df4e4e857a3e
- Child:
- 23:4a490fa8bf4a
diff -r df4e4e857a3e -r da9893b71f24 main.cpp --- a/main.cpp Tue Feb 09 09:03:00 2021 +0000 +++ b/main.cpp Mon Feb 15 06:49:37 2021 +0000 @@ -9,7 +9,6 @@ #define MPU6050_PWR_MGMT_1 0x6B #define MPU_ADDRESS 0x68 #define pi 3.141562 -#define RATE 0.1 // for //加速度 Full-Scale Range //#define ACCEL_FSR MPU6050_ACCEL_FS_2 @@ -52,10 +51,15 @@ // (float Kp, float Ki, float Kd, float tSample); const double PID_dt = 0.01; PID test_control(1.2, 0.0, 0.0, PID_dt); +PID pitchPID(1.0, 0,0,PID_dt); //rad +PID pitchratePID(0.1, 0.0, 0.0, PID_dt);//rad/s +PID rollPID(1.0,0.0,0.0,PID_dt); +PID rollratePID(0.1, 0.0, 0.0, PID_dt);//rad/s Timer t; int ch1, ch2; -float rc1, rc2; +int channels[16]; +//float rc1, rc2; // channels[16]の1,2要素めにそれぞれ移行ずみ double pitch = 0.0; double roll = 0.0; double yaw = 0.0; @@ -76,10 +80,18 @@ { // sd.printf("tstart: %f \n",tstart); } +// 割り込まれた時点での出力(computeの結果)を返す関数 +/* +float PID_compute(){ + +} +*/ int main() { //LoopTicker sdcard_loop; + LoopTicker PIDtick; + //PIDtick.attach(test_control,RATE); //sdcard_loop.attach(pushto_sdcard,0.01); pc.baud(115200); //sd.baud(115200); @@ -103,19 +115,22 @@ MadgwickFilter.begin(100); //サンプリング周波数Hza // のセットアップ test_control.setInputLimits(-60.0,60.0); // 60°が限界 - test_control.setOutputLimits(1200, 1800); - test_control.setSetPoint(0.0); // 仮(実際にはsbusの読み込みなど) - test_control.setBias((1200+1800)/2); // out range の真ん中に設定する(out rangeの中央が安定点のため) + test_control.setOutputLimits(-1,1); t.start(); + float LP_rc = 0.65f; while(1) { tstart = t.read(); //sdcard_loop.loop(); if(sbus.failSafe == false) { - ch1 = int(sbus.getData(1)); - ch2 = int(sbus.getData(2)); - pc.printf("ch1 :%d ", ch1); - - //pc.printf("ch2 :%d ", ch2); + // sbusデータの読み込み + for (int i =0 ; i < 16;i ++){ + int data; + float mapped_data; + data = int(sbus.getData(i)); + mapped_data = LP_rc * float(map(data,368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * channels[i]; // mapped input + channels[i] = int(mapped_data); // channelの値をmapしたものを格納 + } + pc.printf("data: %d",sbus.getData(1)); } // gx gy gz ax ay az @@ -134,40 +149,40 @@ pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180; roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180; - /* - float LP_rc = 0.65f; + //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; - rc2 = LP_rc*float(map(ch2,368,1680,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc2; + //rc1 = LP_rc*float(map(roll * 180.0 / pi,-60,60,-1000,1000))/1000.0f+(1.0f-LP_rc)*rc1; + //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)(rc1*1000.0f),-1000,1000,pwmMin,pwmMax); + out1 = map((int)(channels[1]*1000.0f),-1000,1000,pwmMin,pwmMax); if(out1<pwmMin) { out1 = pwmMin; }; if(out1>pwmMax) { out1 = pwmMax; }; - out2 = map((int)(rc2*1000.0f),-1000,1000,pwmMin,pwmMax); + out2 = map((int)(channels[2]*1000.0f),-1000,1000,pwmMin,pwmMax); if(out2<pwmMin) { out2 = pwmMin; }; if(out2>pwmMax) { out2 = pwmMax; }; - */ - + //test_control.setSetPoint(channels[1]); + test_control.setSetPoint(0.0); // 仮(実際にはsbusの読み込みなど) pc.printf("roll%f ", roll*180.0/pi); - //pc.printf("out1:%d ", out1); + //pc.printf("out1:%d\n ", out1); //pc.printf("out2:%d ", out2); - // pc.printf("%f",PID_dt - t.read() + tstart); wait(PID_dt - t.read() + tstart); // PIDのために、待ち時間調節(割り込みにするべき) float tend = t.read(); 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("co:%d ",out1); + pc.printf("out1:%d ",out1); servoRight.pulsewidth_us(out1); } } \ No newline at end of file