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@35:4535af88f7bf, 2021-03-02 (annotated)
- Committer:
- NaotoMorita
- Date:
- Tue Mar 02 05:01:20 2021 +0000
- Revision:
- 35:4535af88f7bf
- Parent:
- 33:e79457192a4b
- Child:
- 36:e68ce293306e
pulsewidth_us>pulsewidth
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
NaotoMorita | 0:6b18a09a6628 | 1 | #include "mbed.h" |
naker | 20:2c3f113a8e8f | 2 | #include "PIDcontroller.h" |
NaotoMorita | 0:6b18a09a6628 | 3 | #include "SBUS.hpp" |
NaotoMorita | 17:6b7a363d06d2 | 4 | #include <MadgwickAHRS.h> |
cocorlow | 6:2cba569272fe | 5 | #include "LoopTicker.hpp" |
naker | 15:6abaac6e5b03 | 6 | #include "MPU6050.h" |
NaotoMorita | 17:6b7a363d06d2 | 7 | #include <I2Cdev.h> |
NaotoMorita | 35:4535af88f7bf | 8 | |
cocorlow | 6:2cba569272fe | 9 | |
NaotoMorita | 17:6b7a363d06d2 | 10 | #define MPU6050_PWR_MGMT_1 0x6B |
NaotoMorita | 17:6b7a363d06d2 | 11 | #define MPU_ADDRESS 0x68 |
NaotoMorita | 17:6b7a363d06d2 | 12 | #define pi 3.141562 |
NaotoMorita | 17:6b7a363d06d2 | 13 | #define ACCEL_FSR MPU6050_ACCEL_FS_8 |
NaotoMorita | 17:6b7a363d06d2 | 14 | #define ACCEL_SSF 36500.0 |
NaotoMorita | 17:6b7a363d06d2 | 15 | #define GYRO_FSR MPU6050_GYRO_FS_250 |
NaotoMorita | 17:6b7a363d06d2 | 16 | #define GYRO_SSF 131.0 |
NaotoMorita | 17:6b7a363d06d2 | 17 | #define MPU6050_LPF MPU6050_DLPF_BW_256 |
NaotoMorita | 27:43bd0e444633 | 18 | |
NaotoMorita | 17:6b7a363d06d2 | 19 | MPU6050 accelgyro; |
NaotoMorita | 17:6b7a363d06d2 | 20 | Madgwick MadgwickFilter; |
NaotoMorita | 0:6b18a09a6628 | 21 | SBUS sbus(PD_5, PD_6); |
NaotoMorita | 0:6b18a09a6628 | 22 | Serial pc(USBTX, USBRX); |
naker | 26:62d9857eaecc | 23 | Serial sd(PG_14,PG_9); |
cocorlow | 3:79e62f9b13c8 | 24 | DigitalOut led1(LED1); |
cocorlow | 3:79e62f9b13c8 | 25 | DigitalOut led3(LED3); |
NaotoMorita | 27:43bd0e444633 | 26 | |
NaotoMorita | 17:6b7a363d06d2 | 27 | PwmOut servoRight(PE_9); |
NaotoMorita | 32:c4f06cb0e1d6 | 28 | PwmOut servoLeft(PE_11); |
NaotoMorita | 32:c4f06cb0e1d6 | 29 | PwmOut servoThrust(PA_0); |
NaotoMorita | 32:c4f06cb0e1d6 | 30 | const double PID_dt = 1.0/200; |
NaotoMorita | 31:8d02f3b9a067 | 31 | PID pitchPID(3.0, 0,0,PID_dt); //rad |
NaotoMorita | 31:8d02f3b9a067 | 32 | PID pitchratePID(0.01, 0.0, 0.0, PID_dt);//rad/s |
NaotoMorita | 31:8d02f3b9a067 | 33 | PID rollPID(3.0,0.0,0.0,PID_dt); |
NaotoMorita | 31:8d02f3b9a067 | 34 | PID rollratePID(0.01, 0.0, 0.0, PID_dt);//rad/s |
NaotoMorita | 17:6b7a363d06d2 | 35 | Timer t; |
cocorlow | 3:79e62f9b13c8 | 36 | |
NaotoMorita | 27:43bd0e444633 | 37 | int loop_count = 0; |
naker | 23:4a490fa8bf4a | 38 | float rc[16]; |
naker | 25:5aca9b224226 | 39 | float roll_ac; |
naker | 15:6abaac6e5b03 | 40 | double pitch = 0.0; |
naker | 15:6abaac6e5b03 | 41 | double roll = 0.0; |
naker | 15:6abaac6e5b03 | 42 | double yaw = 0.0; |
NaotoMorita | 17:6b7a363d06d2 | 43 | int16_t ax, ay, az; |
NaotoMorita | 17:6b7a363d06d2 | 44 | int16_t gx, gy, gz; |
NaotoMorita | 17:6b7a363d06d2 | 45 | double acc_x,acc_y,acc_z; |
NaotoMorita | 17:6b7a363d06d2 | 46 | double gyro_x,gyro_y,gyro_z; |
cocorlow | 3:79e62f9b13c8 | 47 | int out1, out2; |
NaotoMorita | 28:fc6c46c1db65 | 48 | float scaledServoOut[3]= {0,0,0}; |
NaotoMorita | 28:fc6c46c1db65 | 49 | float scaledMotorOut[1]= {-1}; |
NaotoMorita | 35:4535af88f7bf | 50 | float servoOut[3] = {0.0015,0.0015,0.0015}; |
NaotoMorita | 35:4535af88f7bf | 51 | float motorOut[1] = {0.00095}; |
NaotoMorita | 35:4535af88f7bf | 52 | float servoPwmMax = 0.002; |
NaotoMorita | 35:4535af88f7bf | 53 | float servoPwmMin = 0.0012; |
NaotoMorita | 35:4535af88f7bf | 54 | float motorPwmMax = 0.002; |
NaotoMorita | 35:4535af88f7bf | 55 | float motorPwmMin = 0.00095; |
NaotoMorita | 27:43bd0e444633 | 56 | int offsetVal[6] = {0,0,0,0,0,0}; |
cocorlow | 3:79e62f9b13c8 | 57 | |
NaotoMorita | 17:6b7a363d06d2 | 58 | const double pitch_align = 0.0; |
NaotoMorita | 17:6b7a363d06d2 | 59 | const double roll_align = -0.0; |
NaotoMorita | 27:43bd0e444633 | 60 | |
NaotoMorita | 27:43bd0e444633 | 61 | |
NaotoMorita | 35:4535af88f7bf | 62 | float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) |
cocorlow | 7:70161eb0f854 | 63 | { |
cocorlow | 7:70161eb0f854 | 64 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; |
cocorlow | 7:70161eb0f854 | 65 | } |
NaotoMorita | 27:43bd0e444633 | 66 | |
NaotoMorita | 30:214ddc613a35 | 67 | void writeSdcard() |
naker | 20:2c3f113a8e8f | 68 | { |
NaotoMorita | 35:4535af88f7bf | 69 | pc.printf("motor: %f, servo1: %f, servo2: %f \r\n",motorOut[0],servoOut[0],servoOut[1]); |
naker | 20:2c3f113a8e8f | 70 | } |
NaotoMorita | 27:43bd0e444633 | 71 | |
naker | 22:da9893b71f24 | 72 | // 割り込まれた時点での出力(computeの結果)を返す関数 |
NaotoMorita | 27:43bd0e444633 | 73 | void calcServoOut(){ |
NaotoMorita | 28:fc6c46c1db65 | 74 | if(sbus.failSafe == false) { |
NaotoMorita | 28:fc6c46c1db65 | 75 | // sbusデータの読み込み |
NaotoMorita | 28:fc6c46c1db65 | 76 | for (int i =0 ; i < 16;i ++){ |
NaotoMorita | 35:4535af88f7bf | 77 | rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input |
NaotoMorita | 27:43bd0e444633 | 78 | } |
NaotoMorita | 28:fc6c46c1db65 | 79 | } |
NaotoMorita | 28:fc6c46c1db65 | 80 | |
NaotoMorita | 28:fc6c46c1db65 | 81 | pitch = -MadgwickFilter.getPitchRadians()-pitch_align*pi/180; |
NaotoMorita | 28:fc6c46c1db65 | 82 | roll = -MadgwickFilter.getRollRadians()-roll_align*pi/180; |
NaotoMorita | 28:fc6c46c1db65 | 83 | pitchPID.setProcessValue(pitch); |
NaotoMorita | 28:fc6c46c1db65 | 84 | pitchratePID.setProcessValue(gyro_y); |
NaotoMorita | 28:fc6c46c1db65 | 85 | rollPID.setProcessValue(roll); |
NaotoMorita | 28:fc6c46c1db65 | 86 | rollratePID.setProcessValue(gyro_x); |
NaotoMorita | 28:fc6c46c1db65 | 87 | float de = pitchPID.compute()+pitchratePID.compute()+rc[1]; |
NaotoMorita | 28:fc6c46c1db65 | 88 | float da = rollPID.compute()+rollratePID.compute()+rc[0]; |
NaotoMorita | 28:fc6c46c1db65 | 89 | float dT = rc[2]; |
NaotoMorita | 28:fc6c46c1db65 | 90 | |
NaotoMorita | 28:fc6c46c1db65 | 91 | scaledServoOut[0]=de+da; |
NaotoMorita | 28:fc6c46c1db65 | 92 | scaledServoOut[1]=de-da; |
NaotoMorita | 28:fc6c46c1db65 | 93 | scaledMotorOut[0]= dT; |
NaotoMorita | 31:8d02f3b9a067 | 94 | |
NaotoMorita | 31:8d02f3b9a067 | 95 | for(int i = 0;i<4;i++){ |
NaotoMorita | 35:4535af88f7bf | 96 | servoOut[i] = float(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax)); |
NaotoMorita | 28:fc6c46c1db65 | 97 | if(servoOut[i]<servoPwmMin) { |
NaotoMorita | 28:fc6c46c1db65 | 98 | servoOut[i] = servoPwmMin; |
NaotoMorita | 28:fc6c46c1db65 | 99 | }; |
NaotoMorita | 28:fc6c46c1db65 | 100 | if(servoOut[i]>servoPwmMax) { |
NaotoMorita | 28:fc6c46c1db65 | 101 | servoOut[i] = servoPwmMax; |
NaotoMorita | 28:fc6c46c1db65 | 102 | }; |
NaotoMorita | 28:fc6c46c1db65 | 103 | } |
NaotoMorita | 28:fc6c46c1db65 | 104 | |
NaotoMorita | 31:8d02f3b9a067 | 105 | for(int i = 0;i<1;i++){ |
NaotoMorita | 35:4535af88f7bf | 106 | motorOut[i] = float(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax)); |
NaotoMorita | 28:fc6c46c1db65 | 107 | if(motorOut[i]<motorPwmMin) { |
NaotoMorita | 28:fc6c46c1db65 | 108 | motorOut[i] = motorPwmMin; |
NaotoMorita | 28:fc6c46c1db65 | 109 | }; |
NaotoMorita | 28:fc6c46c1db65 | 110 | if(motorOut[i]>motorPwmMax) { |
NaotoMorita | 28:fc6c46c1db65 | 111 | motorOut[i] = motorPwmMax; |
NaotoMorita | 28:fc6c46c1db65 | 112 | }; |
NaotoMorita | 28:fc6c46c1db65 | 113 | } |
NaotoMorita | 32:c4f06cb0e1d6 | 114 | |
NaotoMorita | 35:4535af88f7bf | 115 | servoRight.pulsewidth(servoOut[0]); |
NaotoMorita | 35:4535af88f7bf | 116 | servoLeft.pulsewidth(servoOut[1]); |
NaotoMorita | 35:4535af88f7bf | 117 | servoThrust.pulsewidth(motorOut[0]); |
NaotoMorita | 32:c4f06cb0e1d6 | 118 | |
NaotoMorita | 31:8d02f3b9a067 | 119 | if(loop_count > int(1/PID_dt/10.0)){ |
NaotoMorita | 31:8d02f3b9a067 | 120 | writeSdcard(); |
naker | 26:62d9857eaecc | 121 | loop_count = 0; |
NaotoMorita | 27:43bd0e444633 | 122 | }else{ |
NaotoMorita | 27:43bd0e444633 | 123 | loop_count +=1; |
naker | 26:62d9857eaecc | 124 | } |
naker | 22:da9893b71f24 | 125 | } |
cocorlow | 7:70161eb0f854 | 126 | |
NaotoMorita | 27:43bd0e444633 | 127 | void updateAttitude(){ |
NaotoMorita | 17:6b7a363d06d2 | 128 | // gx gy gz ax ay az |
NaotoMorita | 17:6b7a363d06d2 | 129 | accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
NaotoMorita | 17:6b7a363d06d2 | 130 | // 加速度値を分解能で割って加速度(G)に変換する |
NaotoMorita | 17:6b7a363d06d2 | 131 | acc_x = ax / ACCEL_SSF; //FS_SEL_0 16,384 LSB / g |
NaotoMorita | 17:6b7a363d06d2 | 132 | acc_y = ay / ACCEL_SSF; |
NaotoMorita | 17:6b7a363d06d2 | 133 | acc_z = az / ACCEL_SSF; |
NaotoMorita | 17:6b7a363d06d2 | 134 | // 角速度値を分解能で割って角速度(deg per sec)に変換する |
NaotoMorita | 17:6b7a363d06d2 | 135 | gyro_x = gx / GYRO_SSF; // (deg/s) |
NaotoMorita | 17:6b7a363d06d2 | 136 | gyro_y = gy / GYRO_SSF; |
NaotoMorita | 17:6b7a363d06d2 | 137 | gyro_z = gz / GYRO_SSF; |
naker | 20:2c3f113a8e8f | 138 | //Madgwickフィルターを用いて、PRY(pitch, roll, yaw)を計算 |
NaotoMorita | 17:6b7a363d06d2 | 139 | MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z); |
NaotoMorita | 27:43bd0e444633 | 140 | } |
NaotoMorita | 27:43bd0e444633 | 141 | |
NaotoMorita | 33:e79457192a4b | 142 | void updateBetweenMeasures(){ |
NaotoMorita | 33:e79457192a4b | 143 | |
NaotoMorita | 33:e79457192a4b | 144 | } |
NaotoMorita | 33:e79457192a4b | 145 | |
NaotoMorita | 27:43bd0e444633 | 146 | int main() |
NaotoMorita | 27:43bd0e444633 | 147 | { |
NaotoMorita | 27:43bd0e444633 | 148 | LoopTicker PIDtick; |
NaotoMorita | 28:fc6c46c1db65 | 149 | PIDtick.attach(calcServoOut,PID_dt); |
NaotoMorita | 32:c4f06cb0e1d6 | 150 | //pc.baud(115200); |
NaotoMorita | 32:c4f06cb0e1d6 | 151 | //sd.baud(115200); |
NaotoMorita | 27:43bd0e444633 | 152 | accelgyro.initialize(); |
NaotoMorita | 27:43bd0e444633 | 153 | //加速度計のフルスケールレンジを設定 |
NaotoMorita | 27:43bd0e444633 | 154 | accelgyro.setFullScaleAccelRange(ACCEL_FSR); |
NaotoMorita | 27:43bd0e444633 | 155 | //■角速度計のフルスケールレンジを設定 |
NaotoMorita | 27:43bd0e444633 | 156 | accelgyro.setFullScaleGyroRange(GYRO_FSR); |
NaotoMorita | 27:43bd0e444633 | 157 | //MPU6050のLPFを設定 |
NaotoMorita | 27:43bd0e444633 | 158 | accelgyro.setDLPFMode(MPU6050_LPF); |
NaotoMorita | 28:fc6c46c1db65 | 159 | |
NaotoMorita | 28:fc6c46c1db65 | 160 | pitchPID.setSetPoint(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 161 | pitchratePID.setSetPoint(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 162 | rollPID.setSetPoint(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 163 | rollratePID.setSetPoint(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 164 | pitchPID.setBias(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 165 | pitchratePID.setBias(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 166 | rollPID.setBias(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 167 | rollratePID.setBias(0.0); |
NaotoMorita | 28:fc6c46c1db65 | 168 | pitchPID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 28:fc6c46c1db65 | 169 | pitchratePID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 28:fc6c46c1db65 | 170 | rollPID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 29:34ee662f454e | 171 | rollratePID.setOutputLimits(-1.0,1.0); |
NaotoMorita | 28:fc6c46c1db65 | 172 | pitchPID.setInputLimits(-pi,pi); |
NaotoMorita | 28:fc6c46c1db65 | 173 | pitchratePID.setInputLimits(-pi,pi); |
NaotoMorita | 28:fc6c46c1db65 | 174 | rollPID.setInputLimits(-pi,pi); |
NaotoMorita | 28:fc6c46c1db65 | 175 | rollratePID.setInputLimits(-pi,pi); |
NaotoMorita | 29:34ee662f454e | 176 | |
NaotoMorita | 35:4535af88f7bf | 177 | servoRight.period(0.02f); |
NaotoMorita | 35:4535af88f7bf | 178 | servoLeft.period(0.02f); |
NaotoMorita | 35:4535af88f7bf | 179 | servoThrust.period(0.02f); |
NaotoMorita | 27:43bd0e444633 | 180 | MadgwickFilter.begin(100); //サンプリング周波数Hza |
NaotoMorita | 27:43bd0e444633 | 181 | t.start(); |
NaotoMorita | 27:43bd0e444633 | 182 | while(1) { |
NaotoMorita | 31:8d02f3b9a067 | 183 | float tstart = t.read(); |
NaotoMorita | 27:43bd0e444633 | 184 | //姿勢角を更新 |
NaotoMorita | 29:34ee662f454e | 185 | updateAttitude(); |
NaotoMorita | 28:fc6c46c1db65 | 186 | PIDtick.loop(); // scaled_servo に pidのcomputing結果を格納する |
naker | 22:da9893b71f24 | 187 | |
NaotoMorita | 17:6b7a363d06d2 | 188 | float tend = t.read(); |
NaotoMorita | 28:fc6c46c1db65 | 189 | MadgwickFilter.begin(1.0f/(tend-tstart)); //サンプリング周波数Hz |
NaotoMorita | 28:fc6c46c1db65 | 190 | |
naker | 20:2c3f113a8e8f | 191 | } |
NaotoMorita | 0:6b18a09a6628 | 192 | } |