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
Diff: main.cpp
- Revision:
- 32:c4f06cb0e1d6
- Parent:
- 31:8d02f3b9a067
- Child:
- 33:e79457192a4b
--- a/main.cpp Tue Feb 16 06:22:16 2021 +0000 +++ b/main.cpp Mon Mar 01 09:08:09 2021 +0000 @@ -24,7 +24,9 @@ DigitalOut led3(LED3); PwmOut servoRight(PE_9); -const double PID_dt = 0.005; +PwmOut servoLeft(PE_11); +PwmOut servoThrust(PA_0); +const double PID_dt = 1.0/200; 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); @@ -49,7 +51,7 @@ int servoPwmMax = 1800; int servoPwmMin = 1200; int motorPwmMax = 2000; -int motorPwmMin = 1000; +int motorPwmMin = 950; int offsetVal[6] = {0,0,0,0,0,0}; const double pitch_align = 0.0; @@ -63,7 +65,7 @@ void writeSdcard() { - sd.printf("pitch: %lf, yaw; %lf, roll: % lf\n",pitch,yaw,roll); + pc.printf("motor: %d, servo1: %d, servo2: %d \r\n",motorOut[0],servoOut[0],servoOut[1]); } // 割り込まれた時点での出力(computeの結果)を返す関数 @@ -108,9 +110,11 @@ motorOut[i] = motorPwmMax; }; } + servoRight.pulsewidth_us(servoOut[0]); - //servoLeft.pulsewidth_us(servoOut[1]); - + servoLeft.pulsewidth_us(servoOut[1]); + servoThrust.pulsewidth_us(motorOut[0]); + if(loop_count > int(1/PID_dt/10.0)){ writeSdcard(); loop_count = 0; @@ -138,8 +142,8 @@ { LoopTicker PIDtick; PIDtick.attach(calcServoOut,PID_dt); - pc.baud(115200); - sd.baud(115200); + //pc.baud(115200); + //sd.baud(115200); accelgyro.initialize(); //加速度計のフルスケールレンジを設定 accelgyro.setFullScaleAccelRange(ACCEL_FSR); @@ -166,6 +170,8 @@ rollratePID.setInputLimits(-pi,pi); servoRight.period_us(20000); + servoLeft.period_us(20000); + servoThrust.period_us(20000); MadgwickFilter.begin(100); //サンプリング周波数Hza t.start(); while(1) {