Eigen Revision
Dependencies: mbed LPS25HB_I2C LSM9DS1 PIDcontroller Autopilot_Eigen LoopTicker GPSUBX_UART_Eigen SBUS_without_mainfile MedianFilter Eigen UsaPack solaESKF_Eigen Vector3 CalibrateMagneto FastPWM
main.cpp
- Committer:
- NaotoMorita
- Date:
- 2021-02-16
- Revision:
- 31:8d02f3b9a067
- Parent:
- 30:214ddc613a35
- Child:
- 32:c4f06cb0e1d6
File content as of revision 31:8d02f3b9a067:
#include "mbed.h" #include "PIDcontroller.h" #include "SBUS.hpp" #include <MadgwickAHRS.h> #include "LoopTicker.hpp" #include "MPU6050.h" #include <I2Cdev.h> #define MPU6050_PWR_MGMT_1 0x6B #define MPU_ADDRESS 0x68 #define pi 3.141562 #define ACCEL_FSR MPU6050_ACCEL_FS_8 #define ACCEL_SSF 36500.0 #define GYRO_FSR MPU6050_GYRO_FS_250 #define GYRO_SSF 131.0 #define MPU6050_LPF MPU6050_DLPF_BW_256 MPU6050 accelgyro; Madgwick MadgwickFilter; SBUS sbus(PD_5, PD_6); Serial pc(USBTX, USBRX); Serial sd(PG_14,PG_9); DigitalOut led1(LED1); DigitalOut led3(LED3); PwmOut servoRight(PE_9); const double PID_dt = 0.005; PID pitchPID(3.0, 0,0,PID_dt); //rad PID pitchratePID(0.01, 0.0, 0.0, PID_dt);//rad/s PID rollPID(3.0,0.0,0.0,PID_dt); PID rollratePID(0.01, 0.0, 0.0, PID_dt);//rad/s Timer t; int loop_count = 0; float rc[16]; float roll_ac; double pitch = 0.0; double roll = 0.0; double yaw = 0.0; int16_t ax, ay, az; int16_t gx, gy, gz; double acc_x,acc_y,acc_z; double gyro_x,gyro_y,gyro_z; int out1, out2; float scaledServoOut[3]= {0,0,0}; float scaledMotorOut[1]= {-1}; int servoOut[3] = {1500,1500,1500}; int motorOut[1] = {1000}; int servoPwmMax = 1800; int servoPwmMin = 1200; int motorPwmMax = 2000; int motorPwmMin = 1000; int offsetVal[6] = {0,0,0,0,0,0}; const double pitch_align = 0.0; const double roll_align = -0.0; 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 writeSdcard() { sd.printf("pitch: %lf, yaw; %lf, roll: % lf\n",pitch,yaw,roll); } // 割り込まれた時点での出力(computeの結果)を返す関数 void calcServoOut(){ if(sbus.failSafe == false) { // sbusデータの読み込み for (int i =0 ; i < 16;i ++){ rc[i] = 0.65f * float(map(int(sbus.getData(i)),368,1680,-1000,1000)) / 1000.0f + (1.0f - 0.65f) * rc[i]; // mapped input } } pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180; roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180; pitchPID.setProcessValue(pitch); pitchratePID.setProcessValue(gyro_y); rollPID.setProcessValue(roll); rollratePID.setProcessValue(gyro_x); float de = pitchPID.compute()+pitchratePID.compute()+rc[1]; float da = rollPID.compute()+rollratePID.compute()+rc[0]; float dT = rc[2]; scaledServoOut[0]=de+da; scaledServoOut[1]=de-da; scaledMotorOut[0]= dT; for(int i = 0;i<4;i++){ servoOut[i] = int(map(scaledServoOut[i]*1000.0,-1000,1000,servoPwmMin,servoPwmMax)); if(servoOut[i]<servoPwmMin) { servoOut[i] = servoPwmMin; }; if(servoOut[i]>servoPwmMax) { servoOut[i] = servoPwmMax; }; } for(int i = 0;i<1;i++){ motorOut[i] = int(map(scaledMotorOut[i]*1000.0,-1000,1000,motorPwmMin,motorPwmMax)); if(motorOut[i]<motorPwmMin) { motorOut[i] = motorPwmMin; }; if(motorOut[i]>motorPwmMax) { motorOut[i] = motorPwmMax; }; } servoRight.pulsewidth_us(servoOut[0]); //servoLeft.pulsewidth_us(servoOut[1]); if(loop_count > int(1/PID_dt/10.0)){ writeSdcard(); loop_count = 0; }else{ loop_count +=1; } } void updateAttitude(){ // gx gy gz ax ay az accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // 加速度値を分解能で割って加速度(G)に変換する acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g acc_y = ay / ACCEL_SSF; acc_z = az / ACCEL_SSF; // 角速度値を分解能で割って角速度(deg per sec)に変換する gyro_x = gx / GYRO_SSF; // (deg/s) gyro_y = gy / GYRO_SSF; gyro_z = gz / GYRO_SSF; //Madgwickフィルターを用いて、PRY(pitch, roll, yaw)を計算 MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z); } int main() { LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); pc.baud(115200); sd.baud(115200); accelgyro.initialize(); //加速度計のフルスケールレンジを設定 accelgyro.setFullScaleAccelRange(ACCEL_FSR); //■角速度計のフルスケールレンジを設定 accelgyro.setFullScaleGyroRange(GYRO_FSR); //MPU6050のLPFを設定 accelgyro.setDLPFMode(MPU6050_LPF); pitchPID.setSetPoint(0.0); pitchratePID.setSetPoint(0.0); rollPID.setSetPoint(0.0); rollratePID.setSetPoint(0.0); pitchPID.setBias(0.0); pitchratePID.setBias(0.0); rollPID.setBias(0.0); rollratePID.setBias(0.0); pitchPID.setOutputLimits(-1.0,1.0); pitchratePID.setOutputLimits(-1.0,1.0); rollPID.setOutputLimits(-1.0,1.0); rollratePID.setOutputLimits(-1.0,1.0); pitchPID.setInputLimits(-pi,pi); pitchratePID.setInputLimits(-pi,pi); rollPID.setInputLimits(-pi,pi); rollratePID.setInputLimits(-pi,pi); servoRight.period_us(20000); MadgwickFilter.begin(100); //サンプリング周波数Hza t.start(); while(1) { float tstart = t.read(); //姿勢角を更新 updateAttitude(); PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する float tend = t.read(); MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hz } }