Quadrirotor
Dependencies: CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed
Fork of Nucleo_MPU_9250 by
CID10DOF/CID10DOF.h
- Committer:
- AlanHuchin
- Date:
- 2018-06-26
- Revision:
- 0:89cf0851969b
File content as of revision 0:89cf0851969b:
/* FreeIMU.h - A libre and easy to use orientation sensing library for Arduino Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net> Development of this code has been supported by the Department of Computer Science, Universita' degli Studi di Torino, Italy within the Piemonte Project http://www.piemonte.di.unito.it/ This program is free software: you can redistribute it and/or modify it under the terms of the version 3 GNU General Public License as published by the Free Software Foundation. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. */ #ifndef CID10DOF_h #define CID10DOF_h // Uncomment the appropriated version of FreeIMU you are using //#define FREEIMU_v01 //#define FREEIMU_v02 //#define FREEIMU_v03 //#define FREEIMU_v035 //#define FREEIMU_v035_MS //#define FREEIMU_v035_BMP #define FREEIMU_v04 #define FREEIMU_LIB_VERSION "20121122" #define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net" #if F_CPU == 16000000L #define FREEIMU_FREQ "16 MHz" #elif F_CPU == 8000000L #define FREEIMU_FREQ "8 MHz" #endif // board IDs #define FREEIMU_ID "Zboub" #define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050)) #define IS_9DOM() #define HAS_AXIS_ALIGNED() (defined(FREEIMU_v01) || defined(FREEIMU_v02) || defined(FREEIMU_v03) || defined(FREEIMU_v035) || defined(FREEIMU_v035_MS) || defined(FREEIMU_v035_BMP) || defined(FREEIMU_v04) || defined(SEN_10121) || defined(SEN_10736)) #include "mbed.h" //#include "calibration.h" /* #ifndef CALIBRATION_H #include <EEPROM.h> #endif #define FREEIMU_EEPROM_BASE 0x0A #define FREEIMU_EEPROM_SIGNATURE 0x19 */ //include des biblis des senseurs #include "MPU9250.h" #include "Matrix.h" #include "Servo.h" #include "PIDcontroller.h" //ESC calibration values #define MAX_TRHUST 1.0 #define MIN_TRHUST 0.45 #define IN_MIN_ROLL -30.0 #define IN_MAX_ROLL 30.0 #define OUT_MIN_ROLL -0.1 #define OUT_MAX_ROLL 1.0 #define IN_MIN_PITCH -30.0 #define IN_MAX_PITCH 30.0 #define OUT_MIN_PITCH -0.1 #define OUT_MAX_PITCH 0.1 #define kp 0.69 //0.82 #define ki 0.0 //0.01 #define kd 0.130//0.17245 #define Td 0.01 #define FIS_TYPE float #define FIS_RESOLUSION 101 #define FIS_MIN -3.4028235E+38 #define FIS_MAX 3.4028235E+38 typedef FIS_TYPE(*_FIS_MF)(FIS_TYPE, FIS_TYPE*); typedef FIS_TYPE(*_FIS_ARR_OP)(FIS_TYPE, FIS_TYPE); typedef FIS_TYPE(*_FIS_ARR)(FIS_TYPE*, int, _FIS_ARR_OP); class CID10DOF { public: CID10DOF(PinName sda, PinName scl,PinName FL, PinName FR, PinName BL, PinName BR); //I2C i2c_; //void init(); //void init(int accgyro_addr, bool fastmode); void MPU9250init(); void MatrixConfig(); #ifndef CALIBRATION_H void calLoad(); #endif void getRawValues(int * raw_values); void MPU9250GetValues(); void MPU9250getYawPitchRoll(); void MPU9250ReadAxis(double * dat); void Calibrate_Motors(); void stabilise(double *speed, double *actSpeed, double *Diff); void setLimits(double min, double max); void run (double *speed); void fis_evaluate(); //PID void PID_Config(double *St_point, double *bias); void PID_Run(double *processVariable, double *setPoint, double *pid_diff); float FLC_roll(float Phi, float iPhi, float dPhi); //float FLC_Roll( float setpoint, float roll_ang, float Ts, float gain); //Sensor de presión y temperatura float getBaroTem(); float getBaroPress(); float getBaroAlt(); double map(double x, double in_min, double in_max, double out_min, double out_max); // we make them public so that users can interact directly with device classes MPU9250 mpu9250; Serial pc; Serial telem; double inMin_, inMax_, inSpan_; double outMin_, outMax_, outSpan_; int16_t accelCount[3]; int16_t gyroCount[3]; int16_t magCount[3]; uint32_t sumCount; int16_t tempCount; float edv, e_ant, P, I, D, PID_fuzz; float temperature, sum; double value, outputs[3]; float yawmag,l; double last_e; float e; char inData[50],byteIn; Servo* motor[4]; private: Timer t; int dt_us; float min_calibrate; //min value at which each motor is calibrated float max_calibrate; //max value ... }; const int fis_gcI = 3; // Number of outputs to the fuzzy inference system const int fis_gcO = 1; // Number of rules to the fuzzy inference system const int fis_gcR = 27; float invSqrt(float number); void arr3_rad_to_deg(float * arr); #endif // FreeIMU_h