Diff: CID10DOF/CID10DOF.h

Revision:

0:89cf0851969b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CID10DOF/CID10DOF.h	Tue Jun 26 18:24:45 2018 +0000
@@ -0,0 +1,187 @@
+/*
+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
\ No newline at end of file