CID10DOF.h

00001 /*
00002 FreeIMU.h - A libre and easy to use orientation sensing library for Arduino
00003 Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net>
00004 
00005 Development of this code has been supported by the Department of Computer Science,
00006 Universita' degli Studi di Torino, Italy within the Piemonte Project
00007 http://www.piemonte.di.unito.it/
00008 
00009 
00010 This program is free software: you can redistribute it and/or modify
00011 it under the terms of the version 3 GNU General Public License as
00012 published by the Free Software Foundation.
00013 
00014 This program is distributed in the hope that it will be useful,
00015 but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017 GNU General Public License for more details.
00018 
00019 You should have received a copy of the GNU General Public License
00020 along with this program.  If not, see <http://www.gnu.org/licenses/>.
00021 
00022 */
00023 
00024 #ifndef CID10DOF_h
00025 #define CID10DOF_h
00026 
00027 // Uncomment the appropriated version of FreeIMU you are using
00028 //#define FREEIMU_v01
00029 //#define FREEIMU_v02
00030 //#define FREEIMU_v03
00031 //#define FREEIMU_v035
00032 //#define FREEIMU_v035_MS
00033 //#define FREEIMU_v035_BMP
00034 #define FREEIMU_v04
00035 
00036 
00037 
00038 #define FREEIMU_LIB_VERSION "20121122"
00039 
00040 #define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net"
00041 
00042 #if F_CPU == 16000000L
00043   #define FREEIMU_FREQ "16 MHz"
00044 #elif F_CPU == 8000000L
00045   #define FREEIMU_FREQ "8 MHz"
00046 #endif
00047 
00048 
00049 // board IDs
00050 
00051 
00052 #define FREEIMU_ID "Zboub"
00053 
00054 #define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050))
00055 #define IS_9DOM() 
00056 #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))
00057 
00058 
00059 
00060 
00061 #include "mbed.h"
00062 
00063 //#include "calibration.h"
00064 /*
00065 #ifndef CALIBRATION_H
00066 #include <EEPROM.h>
00067 #endif
00068 #define FREEIMU_EEPROM_BASE 0x0A
00069 #define FREEIMU_EEPROM_SIGNATURE 0x19
00070 */
00071 
00072 //include des biblis des senseurs
00073 
00074 #include "MPU9250.h"
00075 #include "Matrix.h"
00076 #include "Servo.h" 
00077 #include "PIDcontroller.h"
00078  
00079 //ESC calibration values
00080 #define MAX_TRHUST 1.0
00081 #define MIN_TRHUST 0.45
00082 
00083 #define IN_MIN_ROLL -30.0
00084 #define IN_MAX_ROLL 30.0
00085 #define OUT_MIN_ROLL  -0.1
00086 #define OUT_MAX_ROLL  1.0
00087 
00088 #define IN_MIN_PITCH  -30.0
00089 #define IN_MAX_PITCH  30.0
00090 #define OUT_MIN_PITCH -0.1
00091 #define OUT_MAX_PITCH 0.1
00092 
00093 #define kp 0.69 //0.82
00094 #define ki 0.0 //0.01
00095 #define kd 0.130//0.17245 
00096 #define Td 0.01
00097 
00098 #define FIS_TYPE float
00099 #define FIS_RESOLUSION 101
00100 #define FIS_MIN -3.4028235E+38
00101 #define FIS_MAX 3.4028235E+38
00102 
00103 typedef FIS_TYPE(*_FIS_MF)(FIS_TYPE, FIS_TYPE*);
00104 typedef FIS_TYPE(*_FIS_ARR_OP)(FIS_TYPE, FIS_TYPE);
00105 typedef FIS_TYPE(*_FIS_ARR)(FIS_TYPE*, int, _FIS_ARR_OP);
00106 
00107 
00108 
00109 class CID10DOF
00110 {
00111   public:
00112     CID10DOF(PinName sda, PinName scl,PinName FL, PinName FR, PinName BL, PinName BR);
00113     //I2C i2c_;
00114     
00115     //void init();
00116     //void init(int accgyro_addr, bool fastmode);
00117     void MPU9250init();
00118     void MatrixConfig();
00119     #ifndef CALIBRATION_H
00120     void calLoad();
00121     #endif
00122     void getRawValues(int * raw_values);
00123     void MPU9250GetValues();
00124     void MPU9250getYawPitchRoll();
00125     void MPU9250ReadAxis(double * dat);
00126     
00127     void Calibrate_Motors();
00128     void stabilise(double *speed, double *actSpeed, double *Diff);
00129     void setLimits(double min, double max);
00130     void run (double *speed);
00131     void fis_evaluate();
00132     
00133     //PID
00134     void PID_Config(double *St_point, double *bias);
00135     void PID_Run(double *processVariable, double *setPoint, double *pid_diff);
00136         
00137     float FLC_roll(float Phi, float iPhi, float dPhi);
00138     //float FLC_Roll( float setpoint, float roll_ang, float Ts, float gain);
00139     
00140     //Sensor de presión y temperatura
00141     float getBaroTem();
00142     float getBaroPress();
00143     float getBaroAlt();
00144     
00145     double map(double x, double in_min, double in_max, double out_min, double out_max);
00146     
00147     // we make them public so that users can interact directly with device classes
00148     
00149     MPU9250 mpu9250;
00150     Serial pc;
00151     Serial telem;
00152     
00153     double  inMin_,  inMax_,  inSpan_; 
00154     double outMin_, outMax_, outSpan_; 
00155     
00156     int16_t accelCount[3];  
00157     int16_t gyroCount[3];  
00158     int16_t magCount[3];
00159     uint32_t sumCount;
00160     int16_t tempCount;
00161     float edv, e_ant, P, I, D, PID_fuzz;  
00162     float temperature, sum;
00163     double value, outputs[3];
00164     float yawmag,l;
00165     double last_e;
00166     float e;
00167     char  inData[50],byteIn;
00168     
00169     Servo* motor[4];
00170     
00171   private:
00172     Timer t;
00173     int dt_us; 
00174     
00175     float min_calibrate;    //min value at which each motor is calibrated
00176     float max_calibrate;    //max value ...
00177  
00178 };
00179     const int fis_gcI = 3;
00180     // Number of outputs to the fuzzy inference system
00181     const int fis_gcO = 1;
00182     // Number of rules to the fuzzy inference system
00183     const int fis_gcR = 27;
00184     
00185     float invSqrt(float number);
00186     void arr3_rad_to_deg(float * arr);
00187 #endif // FreeIMU_h