Alan Huchin Herrera / Mbed 2 deprecated Nucleo_MPU_9250_

Quadrirotor

Dependencies:   CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed

Fork of Nucleo_MPU_9250 by Alan Huchin Herrera

CID10DOF/CID10DOF.h@0:89cf0851969b, 2018-06-26 (annotated)

Committer:
AlanHuchin
Date:
Tue Jun 26 18:24:45 2018 +0000
Revision:
0:89cf0851969b
hello

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AlanHuchin 0:89cf0851969b 1 /*
AlanHuchin 0:89cf0851969b 2 FreeIMU.h - A libre and easy to use orientation sensing library for Arduino
AlanHuchin 0:89cf0851969b 3 Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net>
AlanHuchin 0:89cf0851969b 4
AlanHuchin 0:89cf0851969b 5 Development of this code has been supported by the Department of Computer Science,
AlanHuchin 0:89cf0851969b 6 Universita' degli Studi di Torino, Italy within the Piemonte Project
AlanHuchin 0:89cf0851969b 7 http://www.piemonte.di.unito.it/
AlanHuchin 0:89cf0851969b 8
AlanHuchin 0:89cf0851969b 9
AlanHuchin 0:89cf0851969b 10 This program is free software: you can redistribute it and/or modify
AlanHuchin 0:89cf0851969b 11 it under the terms of the version 3 GNU General Public License as
AlanHuchin 0:89cf0851969b 12 published by the Free Software Foundation.
AlanHuchin 0:89cf0851969b 13
AlanHuchin 0:89cf0851969b 14 This program is distributed in the hope that it will be useful,
AlanHuchin 0:89cf0851969b 15 but WITHOUT ANY WARRANTY; without even the implied warranty of
AlanHuchin 0:89cf0851969b 16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
AlanHuchin 0:89cf0851969b 17 GNU General Public License for more details.
AlanHuchin 0:89cf0851969b 18
AlanHuchin 0:89cf0851969b 19 You should have received a copy of the GNU General Public License
AlanHuchin 0:89cf0851969b 20 along with this program. If not, see <http://www.gnu.org/licenses/>.
AlanHuchin 0:89cf0851969b 21
AlanHuchin 0:89cf0851969b 22 */
AlanHuchin 0:89cf0851969b 23
AlanHuchin 0:89cf0851969b 24 #ifndef CID10DOF_h
AlanHuchin 0:89cf0851969b 25 #define CID10DOF_h
AlanHuchin 0:89cf0851969b 26
AlanHuchin 0:89cf0851969b 27 // Uncomment the appropriated version of FreeIMU you are using
AlanHuchin 0:89cf0851969b 28 //#define FREEIMU_v01
AlanHuchin 0:89cf0851969b 29 //#define FREEIMU_v02
AlanHuchin 0:89cf0851969b 30 //#define FREEIMU_v03
AlanHuchin 0:89cf0851969b 31 //#define FREEIMU_v035
AlanHuchin 0:89cf0851969b 32 //#define FREEIMU_v035_MS
AlanHuchin 0:89cf0851969b 33 //#define FREEIMU_v035_BMP
AlanHuchin 0:89cf0851969b 34 #define FREEIMU_v04
AlanHuchin 0:89cf0851969b 35
AlanHuchin 0:89cf0851969b 36
AlanHuchin 0:89cf0851969b 37
AlanHuchin 0:89cf0851969b 38 #define FREEIMU_LIB_VERSION "20121122"
AlanHuchin 0:89cf0851969b 39
AlanHuchin 0:89cf0851969b 40 #define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net"
AlanHuchin 0:89cf0851969b 41
AlanHuchin 0:89cf0851969b 42 #if F_CPU == 16000000L
AlanHuchin 0:89cf0851969b 43 #define FREEIMU_FREQ "16 MHz"
AlanHuchin 0:89cf0851969b 44 #elif F_CPU == 8000000L
AlanHuchin 0:89cf0851969b 45 #define FREEIMU_FREQ "8 MHz"
AlanHuchin 0:89cf0851969b 46 #endif
AlanHuchin 0:89cf0851969b 47
AlanHuchin 0:89cf0851969b 48
AlanHuchin 0:89cf0851969b 49 // board IDs
AlanHuchin 0:89cf0851969b 50
AlanHuchin 0:89cf0851969b 51
AlanHuchin 0:89cf0851969b 52 #define FREEIMU_ID "Zboub"
AlanHuchin 0:89cf0851969b 53
AlanHuchin 0:89cf0851969b 54 #define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050))
AlanHuchin 0:89cf0851969b 55 #define IS_9DOM()
AlanHuchin 0:89cf0851969b 56 #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))
AlanHuchin 0:89cf0851969b 57
AlanHuchin 0:89cf0851969b 58
AlanHuchin 0:89cf0851969b 59
AlanHuchin 0:89cf0851969b 60
AlanHuchin 0:89cf0851969b 61 #include "mbed.h"
AlanHuchin 0:89cf0851969b 62
AlanHuchin 0:89cf0851969b 63 //#include "calibration.h"
AlanHuchin 0:89cf0851969b 64 /*
AlanHuchin 0:89cf0851969b 65 #ifndef CALIBRATION_H
AlanHuchin 0:89cf0851969b 66 #include <EEPROM.h>
AlanHuchin 0:89cf0851969b 67 #endif
AlanHuchin 0:89cf0851969b 68 #define FREEIMU_EEPROM_BASE 0x0A
AlanHuchin 0:89cf0851969b 69 #define FREEIMU_EEPROM_SIGNATURE 0x19
AlanHuchin 0:89cf0851969b 70 */
AlanHuchin 0:89cf0851969b 71
AlanHuchin 0:89cf0851969b 72 //include des biblis des senseurs
AlanHuchin 0:89cf0851969b 73
AlanHuchin 0:89cf0851969b 74 #include "MPU9250.h"
AlanHuchin 0:89cf0851969b 75 #include "Matrix.h"
AlanHuchin 0:89cf0851969b 76 #include "Servo.h"
AlanHuchin 0:89cf0851969b 77 #include "PIDcontroller.h"
AlanHuchin 0:89cf0851969b 78
AlanHuchin 0:89cf0851969b 79 //ESC calibration values
AlanHuchin 0:89cf0851969b 80 #define MAX_TRHUST 1.0
AlanHuchin 0:89cf0851969b 81 #define MIN_TRHUST 0.45
AlanHuchin 0:89cf0851969b 82
AlanHuchin 0:89cf0851969b 83 #define IN_MIN_ROLL -30.0
AlanHuchin 0:89cf0851969b 84 #define IN_MAX_ROLL 30.0
AlanHuchin 0:89cf0851969b 85 #define OUT_MIN_ROLL -0.1
AlanHuchin 0:89cf0851969b 86 #define OUT_MAX_ROLL 1.0
AlanHuchin 0:89cf0851969b 87
AlanHuchin 0:89cf0851969b 88 #define IN_MIN_PITCH -30.0
AlanHuchin 0:89cf0851969b 89 #define IN_MAX_PITCH 30.0
AlanHuchin 0:89cf0851969b 90 #define OUT_MIN_PITCH -0.1
AlanHuchin 0:89cf0851969b 91 #define OUT_MAX_PITCH 0.1
AlanHuchin 0:89cf0851969b 92
AlanHuchin 0:89cf0851969b 93 #define kp 0.69 //0.82
AlanHuchin 0:89cf0851969b 94 #define ki 0.0 //0.01
AlanHuchin 0:89cf0851969b 95 #define kd 0.130//0.17245
AlanHuchin 0:89cf0851969b 96 #define Td 0.01
AlanHuchin 0:89cf0851969b 97
AlanHuchin 0:89cf0851969b 98 #define FIS_TYPE float
AlanHuchin 0:89cf0851969b 99 #define FIS_RESOLUSION 101
AlanHuchin 0:89cf0851969b 100 #define FIS_MIN -3.4028235E+38
AlanHuchin 0:89cf0851969b 101 #define FIS_MAX 3.4028235E+38
AlanHuchin 0:89cf0851969b 102
AlanHuchin 0:89cf0851969b 103 typedef FIS_TYPE(*_FIS_MF)(FIS_TYPE, FIS_TYPE*);
AlanHuchin 0:89cf0851969b 104 typedef FIS_TYPE(*_FIS_ARR_OP)(FIS_TYPE, FIS_TYPE);
AlanHuchin 0:89cf0851969b 105 typedef FIS_TYPE(*_FIS_ARR)(FIS_TYPE*, int, _FIS_ARR_OP);
AlanHuchin 0:89cf0851969b 106
AlanHuchin 0:89cf0851969b 107
AlanHuchin 0:89cf0851969b 108
AlanHuchin 0:89cf0851969b 109 class CID10DOF
AlanHuchin 0:89cf0851969b 110 {
AlanHuchin 0:89cf0851969b 111 public:
AlanHuchin 0:89cf0851969b 112 CID10DOF(PinName sda, PinName scl,PinName FL, PinName FR, PinName BL, PinName BR);
AlanHuchin 0:89cf0851969b 113 //I2C i2c_;
AlanHuchin 0:89cf0851969b 114
AlanHuchin 0:89cf0851969b 115 //void init();
AlanHuchin 0:89cf0851969b 116 //void init(int accgyro_addr, bool fastmode);
AlanHuchin 0:89cf0851969b 117 void MPU9250init();
AlanHuchin 0:89cf0851969b 118 void MatrixConfig();
AlanHuchin 0:89cf0851969b 119 #ifndef CALIBRATION_H
AlanHuchin 0:89cf0851969b 120 void calLoad();
AlanHuchin 0:89cf0851969b 121 #endif
AlanHuchin 0:89cf0851969b 122 void getRawValues(int * raw_values);
AlanHuchin 0:89cf0851969b 123 void MPU9250GetValues();
AlanHuchin 0:89cf0851969b 124 void MPU9250getYawPitchRoll();
AlanHuchin 0:89cf0851969b 125 void MPU9250ReadAxis(double * dat);
AlanHuchin 0:89cf0851969b 126
AlanHuchin 0:89cf0851969b 127 void Calibrate_Motors();
AlanHuchin 0:89cf0851969b 128 void stabilise(double *speed, double *actSpeed, double *Diff);
AlanHuchin 0:89cf0851969b 129 void setLimits(double min, double max);
AlanHuchin 0:89cf0851969b 130 void run (double *speed);
AlanHuchin 0:89cf0851969b 131 void fis_evaluate();
AlanHuchin 0:89cf0851969b 132
AlanHuchin 0:89cf0851969b 133 //PID
AlanHuchin 0:89cf0851969b 134 void PID_Config(double *St_point, double *bias);
AlanHuchin 0:89cf0851969b 135 void PID_Run(double *processVariable, double *setPoint, double *pid_diff);
AlanHuchin 0:89cf0851969b 136
AlanHuchin 0:89cf0851969b 137 float FLC_roll(float Phi, float iPhi, float dPhi);
AlanHuchin 0:89cf0851969b 138 //float FLC_Roll( float setpoint, float roll_ang, float Ts, float gain);
AlanHuchin 0:89cf0851969b 139
AlanHuchin 0:89cf0851969b 140 //Sensor de presión y temperatura
AlanHuchin 0:89cf0851969b 141 float getBaroTem();
AlanHuchin 0:89cf0851969b 142 float getBaroPress();
AlanHuchin 0:89cf0851969b 143 float getBaroAlt();
AlanHuchin 0:89cf0851969b 144
AlanHuchin 0:89cf0851969b 145 double map(double x, double in_min, double in_max, double out_min, double out_max);
AlanHuchin 0:89cf0851969b 146
AlanHuchin 0:89cf0851969b 147 // we make them public so that users can interact directly with device classes
AlanHuchin 0:89cf0851969b 148
AlanHuchin 0:89cf0851969b 149 MPU9250 mpu9250;
AlanHuchin 0:89cf0851969b 150 Serial pc;
AlanHuchin 0:89cf0851969b 151 Serial telem;
AlanHuchin 0:89cf0851969b 152
AlanHuchin 0:89cf0851969b 153 double inMin_, inMax_, inSpan_;
AlanHuchin 0:89cf0851969b 154 double outMin_, outMax_, outSpan_;
AlanHuchin 0:89cf0851969b 155
AlanHuchin 0:89cf0851969b 156 int16_t accelCount[3];
AlanHuchin 0:89cf0851969b 157 int16_t gyroCount[3];
AlanHuchin 0:89cf0851969b 158 int16_t magCount[3];
AlanHuchin 0:89cf0851969b 159 uint32_t sumCount;
AlanHuchin 0:89cf0851969b 160 int16_t tempCount;
AlanHuchin 0:89cf0851969b 161 float edv, e_ant, P, I, D, PID_fuzz;
AlanHuchin 0:89cf0851969b 162 float temperature, sum;
AlanHuchin 0:89cf0851969b 163 double value, outputs[3];
AlanHuchin 0:89cf0851969b 164 float yawmag,l;
AlanHuchin 0:89cf0851969b 165 double last_e;
AlanHuchin 0:89cf0851969b 166 float e;
AlanHuchin 0:89cf0851969b 167 char inData[50],byteIn;
AlanHuchin 0:89cf0851969b 168
AlanHuchin 0:89cf0851969b 169 Servo* motor[4];
AlanHuchin 0:89cf0851969b 170
AlanHuchin 0:89cf0851969b 171 private:
AlanHuchin 0:89cf0851969b 172 Timer t;
AlanHuchin 0:89cf0851969b 173 int dt_us;
AlanHuchin 0:89cf0851969b 174
AlanHuchin 0:89cf0851969b 175 float min_calibrate; //min value at which each motor is calibrated
AlanHuchin 0:89cf0851969b 176 float max_calibrate; //max value ...
AlanHuchin 0:89cf0851969b 177
AlanHuchin 0:89cf0851969b 178 };
AlanHuchin 0:89cf0851969b 179 const int fis_gcI = 3;
AlanHuchin 0:89cf0851969b 180 // Number of outputs to the fuzzy inference system
AlanHuchin 0:89cf0851969b 181 const int fis_gcO = 1;
AlanHuchin 0:89cf0851969b 182 // Number of rules to the fuzzy inference system
AlanHuchin 0:89cf0851969b 183 const int fis_gcR = 27;
AlanHuchin 0:89cf0851969b 184
AlanHuchin 0:89cf0851969b 185 float invSqrt(float number);
AlanHuchin 0:89cf0851969b 186 void arr3_rad_to_deg(float * arr);
AlanHuchin 0:89cf0851969b 187 #endif // FreeIMU_h