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