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:
- 35:4535af88f7bf
- Parent:
- 33:e79457192a4b
- Child:
- 36:e68ce293306e
--- a/main.cpp Tue Mar 02 04:29:30 2021 +0000 +++ b/main.cpp Tue Mar 02 05:01:20 2021 +0000 @@ -5,7 +5,7 @@ #include "LoopTicker.hpp" #include "MPU6050.h" #include <I2Cdev.h> -#include "Matrix.h" + #define MPU6050_PWR_MGMT_1 0x6B #define MPU_ADDRESS 0x68 @@ -32,10 +32,6 @@ 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 -Quaternion qhat(1,0,0,0); -Matrix Phat(4,4); -Matrix Qgyro(3,3); -Matrix Racc(3,3); Timer t; int loop_count = 0; @@ -51,26 +47,26 @@ 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 = 950; +float servoOut[3] = {0.0015,0.0015,0.0015}; +float motorOut[1] = {0.00095}; +float servoPwmMax = 0.002; +float servoPwmMin = 0.0012; +float motorPwmMax = 0.002; +float motorPwmMin = 0.00095; 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) +float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } void writeSdcard() { - pc.printf("motor: %d, servo1: %d, servo2: %d \r\n",motorOut[0],servoOut[0],servoOut[1]); + pc.printf("motor: %f, servo1: %f, servo2: %f \r\n",motorOut[0],servoOut[0],servoOut[1]); } // 割り込まれた時点での出力(computeの結果)を返す関数 @@ -78,7 +74,7 @@ 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 + rc[i] = 0.65f * mapfloat(float(sbus.getData(i)),368,1680,-1,1) + (1.0f - 0.65f) * rc[i]; // mapped input } } @@ -97,7 +93,7 @@ scaledMotorOut[0]= dT; for(int i = 0;i<4;i++){ - servoOut[i] = int(map(scaledServoOut[i]*1000.0,-1000,1000,servoPwmMin,servoPwmMax)); + servoOut[i] = float(mapfloat(scaledServoOut[i],-1,1,servoPwmMin,servoPwmMax)); if(servoOut[i]<servoPwmMin) { servoOut[i] = servoPwmMin; }; @@ -107,7 +103,7 @@ } for(int i = 0;i<1;i++){ - motorOut[i] = int(map(scaledMotorOut[i]*1000.0,-1000,1000,motorPwmMin,motorPwmMax)); + motorOut[i] = float(mapfloat(scaledMotorOut[i],-1,1,motorPwmMin,motorPwmMax)); if(motorOut[i]<motorPwmMin) { motorOut[i] = motorPwmMin; }; @@ -116,9 +112,9 @@ }; } - servoRight.pulsewidth_us(servoOut[0]); - servoLeft.pulsewidth_us(servoOut[1]); - servoThrust.pulsewidth_us(motorOut[0]); + servoRight.pulsewidth(servoOut[0]); + servoLeft.pulsewidth(servoOut[1]); + servoThrust.pulsewidth(motorOut[0]); if(loop_count > int(1/PID_dt/10.0)){ writeSdcard(); @@ -178,9 +174,9 @@ rollPID.setInputLimits(-pi,pi); rollratePID.setInputLimits(-pi,pi); - servoRight.period_us(20000); - servoLeft.period_us(20000); - servoThrust.period_us(20000); + servoRight.period(0.02f); + servoLeft.period(0.02f); + servoThrust.period(0.02f); MadgwickFilter.begin(100); //サンプリング周波数Hza t.start(); while(1) {