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:
- 20:2c3f113a8e8f
- Parent:
- 19:61d7e9b4b6c2
- Child:
- 21:df4e4e857a3e
--- a/main.cpp Fri Feb 05 04:58:07 2021 +0000 +++ b/main.cpp Tue Feb 09 03:31:18 2021 +0000 @@ -1,14 +1,15 @@ #include "mbed.h" +#include "PIDcontroller.h" #include "SBUS.hpp" #include <MadgwickAHRS.h> #include "LoopTicker.hpp" #include "MPU6050.h" #include <I2Cdev.h> -//#include <Servo.h> #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 @@ -29,23 +30,28 @@ //#define GYRO_SSF 16.4 #define MPU6050_LPF MPU6050_DLPF_BW_256 //#define MPU6050_LPF MPU6050_DLPF_BW_188 -//#define MPU6050_LPF MPU6050_DLPF_BW_98 -//#define MPU6050_LPF MPU6050_DLPF_BW_42 -//#define MPU6050_LPF MPU6050_DLPF_BW_20 -//#define MPU6050_LPF MPU6050_DLPF_BW_10 -//#define MPU6050_LPF MPU6050_DLPF_BW_5 +//#define MPU6050_LPF MPU6050_DLPF_BW_98 +//#define MPU6050_LPF MPU6050_DLPF_BW_42 +//#define MPU6050_LPF MPU6050_DLPF_BW_20 +//#define MPU6050_LPF MPU6050_DLPF_BW_10 +//#define MPU6050_LPF MPU6050_DLPF_BW_5 MPU6050 accelgyro; 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); -//Servo servoRight(PB_4); //右のサーボ +//PwmOut co(PE_9); // サーボのoutput +//Servo servoRight(PE_); //右のサーボ //Servo servoLeft(PB_5); //左のサーボ //Servo thrServo(PB_6); //スロットルサーボ +// のセットアップ +// (float Kp, float Ki, float Kd, float tSample); +const double PID_dt = 0.01; +PID test_control(0.1, 0.1, 0.1, PID_dt); Timer t; int ch1, ch2; @@ -61,17 +67,22 @@ const double pitch_align = 0.0; const double roll_align = -0.0; - +float tstart; long map(long x, long in_min, long in_max, long out_min, long out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } +void pushto_sdcard() +{ + // sd.printf("tstart: %f \n",tstart); +} int main() { - int count = 0; + //LoopTicker sdcard_loop; + //sdcard_loop.attach(pushto_sdcard,0.01); pc.baud(115200); - sd.baud(9600); + //sd.baud(115200); accelgyro.initialize(); uint8_t offsetVal[6] = {0,0,0,0,0,0}; accelgyro.setXAccelOffset(offsetVal[0]); @@ -86,40 +97,26 @@ accelgyro.setFullScaleGyroRange(GYRO_FSR); //MPU6050のLPFを設定 accelgyro.setDLPFMode(MPU6050_LPF); - + servoRight.period_us(20000); - + MadgwickFilter.begin(100); //サンプリング周波数Hza + // のセットアップ + test_control.setInputLimits(-60.0,60.0); // 60°が限界 + test_control.setOutputLimits(1200, 1800); + test_control.setSetPoint(0); // 仮(実際にはsbusの読み込みなど) t.start(); while(1) { - if(count == 50){ - sd.printf("test"); - count = 0; - } - count ++; - float tstart = t.read(); - if(sbus.failSafe == false) - { + 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("ch1 :%d ", ch1); + //pc.printf("ch2 :%d ", ch2); - float LP_rc = 0.65f; - float LP_rc3 = 0.15f; - rc1 = LP_rc*float(map(ch1,368,1680,-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; - //pc.printf("rc1 :%f ", rc1); - //pc.printf("rc2 :%f ", rc1); - int pwmMax = 1800; - int pwmMin = 1200; - out1 = map((int)(rc1*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); - if(out2<pwmMin){out2 = pwmMin;}; - if(out2>pwmMax){out2 = pwmMax;}; } - + // gx gy gz ax ay az accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // 加速度値を分解能で割って加速度(G)に変換する @@ -130,24 +127,46 @@ gyro_x = gx / GYRO_SSF; // (deg/s) gyro_y = gy / GYRO_SSF; gyro_z = gz / GYRO_SSF; - //Madgwickフィルターを用いて、PRY(pitch, roll, yaw)を計算 + //Madgwickフィルターを用いて、PRY(pitch, roll, yaw)を計算 //pc.printf("accx:%f accy:%f accz:%f \r\n", acc_x,acc_y,acc_z); MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z); pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180; roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180; - - servoRight.pulsewidth_us(out1); + /* + 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; + int pwmMax = 1800; + int pwmMin = 1200; + out1 = map((int)(rc1*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); + if(out2<pwmMin) { + out2 = pwmMin; + }; + if(out2>pwmMax) { + out2 = pwmMax; + }; + */ - pc.printf("out1:%d ", out1); - //pc.printf("roll%f ", roll*180.0/pi); + pc.printf("roll%f ", roll*180.0/pi); //pc.printf("out1:%d ", 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(); - wait(0.005); 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); + servoRight.pulsewidth_us(out1); + } } \ No newline at end of file