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:
- 25:5aca9b224226
- Parent:
- 24:6ab61527855d
- Child:
- 26:62d9857eaecc
diff -r 6ab61527855d -r 5aca9b224226 main.cpp --- a/main.cpp Mon Feb 15 07:14:44 2021 +0000 +++ b/main.cpp Mon Feb 15 08:30:16 2021 +0000 @@ -38,7 +38,7 @@ Madgwick MadgwickFilter; SBUS sbus(PD_5, PD_6); Serial pc(USBTX, USBRX); -// Serial sd(PE_8,PE_7); +Serial sd(PE_8,PE_7); DigitalOut led1(LED1); DigitalOut led3(LED3); PwmOut servoRight(PE_9); @@ -60,6 +60,7 @@ int ch1, ch2; float rc[16]; //float rc1, rc2; // rc[16]の1,2要素めにそれぞれ移行ずみ +float roll_ac; double pitch = 0.0; double roll = 0.0; double yaw = 0.0; @@ -68,7 +69,7 @@ double acc_x,acc_y,acc_z; double gyro_x,gyro_y,gyro_z; int out1, out2; -int scaler_servo[3]; +float scaled_servo[3]; int servo_out[3]; const double pitch_align = 0.0; @@ -80,23 +81,22 @@ } void pushto_sdcard() { - // sd.printf("tstart: %f \n",tstart); + sd.printf("tstart: %f \n",tstart); } // 割り込まれた時点での出力(computeの結果)を返す関数 -/* -float PID_compute(){ - + +void PID_compute(){ + scaled_servo[0] = test_control.compute(); } -*/ int main() { - //LoopTicker sdcard_loop; + LoopTicker sdcard_loop; LoopTicker PIDtick; - //PIDtick.attach(test_control,PID_dt); - //sdcard_loop.attach(pushto_sdcard,0.01); + PIDtick.attach(PID_compute,PID_dt); + sdcard_loop.attach(pushto_sdcard,1.0); pc.baud(115200); - //sd.baud(115200); + sd.baud(115200); accelgyro.initialize(); uint8_t offsetVal[6] = {0,0,0,0,0,0}; accelgyro.setXAccelOffset(offsetVal[0]); @@ -117,22 +117,23 @@ MadgwickFilter.begin(100); //サンプリング周波数Hza // のセットアップ test_control.setInputLimits(-60.0,60.0); // 60°が限界 - test_control.setOutputLimits(1200, 1800); - test_control.setBias((1200+1800)/2); // out range の真ん中に設定する(out rangeの中央が安定点のため) + test_control.setOutputLimits(-1, 1); + test_control.setBias(0.0); t.start(); float LP_rc = 0.65f; while(1) { tstart = t.read(); - //sdcard_loop.loop(); + sdcard_loop.loop(); if(sbus.failSafe == false) { // sbusデータの読み込み for (int i =0 ; i < 16;i ++){ - int data; - float mapped_data; rc[i] = LP_rc * float(map(int(sbus.getData(i)),368,1680,-1000,1000)) / 1000.0f + (1.0f - LP_rc) * rc[i]; // mapped input } - pc.printf("data: %d",rc[1]); + float rc_setpoint = map((int)(rc[1]*1000.0f),-1000,1000,-30,30); // sbus用 + pc.printf("data: %f",rc_setpoint); + test_control.setSetPoint(rc_setpoint); } + else test_control.setSetPoint(0.0); // gx gy gz ax ay az accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); @@ -153,12 +154,9 @@ //float LP_rc3 = 0.15f; 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)(de _left*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用 + //out1 = map((int)(rc[1]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用 if(out1<pwmMin) { out1 = pwmMin; }; @@ -172,8 +170,7 @@ if(out2>pwmMax) { out2 = pwmMax; }; - //test_control.setSetPoint(rc[1]); - test_control.setSetPoint(0.0); // 仮(実際にはsbusの読み込みなど) + //test_control.setSetPoint(0.0); // 仮(実際にはsbusの読み込みなど) pc.printf("roll%f ", roll*180.0/pi); //pc.printf("out1:%d\n ", out1); //pc.printf("out2:%d ", out2); @@ -182,28 +179,14 @@ MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hza pc.printf("time%f \r\n", (tend-tstart)); test_control.setProcessValue(roll * 180.0 / pi); // 入力はこどほう - scaled_servo[0] = test_control.compute(); // -1 ~ 1 - servo_out[0] = map((int)(scaler_servo[0]*1000.0f),-1000,1000,pwmMin,pwmMax); // sbus用 + PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する + printf("scaled_servo[0]: %f",scaled_servo[0]); + servo_out[0] = map((int)(scaled_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] = max(servo_out[i],pwmMin); 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]); + pc.printf("out1:%d ",servo_out[0]); + servoRight.pulsewidth_us(servo_out[0]); } } \ No newline at end of file