David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

quadcopter.h@32:e12b01c94b4a, 2016-04-22 (annotated)

Committer:
ivo_david_michelle
Date:
Fri Apr 22 00:52:32 2016 +0000
Revision:
32:e12b01c94b4a
Parent:
31:d473eacfc271
Child:
34:eaea0ae92dfa
reasonably ok control gains

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivo_david_michelle 6:6f3ffd97d808 1 #ifndef QUADCOPTER_H
ivo_david_michelle 6:6f3ffd97d808 2 #define QUADCOPTER_H
ivo_david_michelle 6:6f3ffd97d808 3
ivo_david_michelle 7:f3f94eadc5b5 4 #include "mbed.h"
ivo_david_michelle 7:f3f94eadc5b5 5 #include "Adafruit_9DOF.h"
ivo_david_michelle 9:f1bd96708a21 6 #include "Serial_base.h"
ivo_david_michelle 14:64b06476d943 7 #include "MRF24J40.h"
ivo_david_michelle 14:64b06476d943 8
ivo_david_michelle 32:e12b01c94b4a 9 #define FILTER_SIZE 3
ivo_david_michelle 32:e12b01c94b4a 10
ivo_david_michelle 13:291ba30c7806 11 // a few struct declarations. Can possibly eliminated using the vector class in STL
ivo_david_michelle 7:f3f94eadc5b5 12 struct state {
ivo_david_michelle 10:e7d1801e966a 13 double phi; // roll
ivo_david_michelle 10:e7d1801e966a 14 double theta; // pitch
ivo_david_michelle 10:e7d1801e966a 15 double psi; // yaw
ivo_david_michelle 10:e7d1801e966a 16 double p; // omega x
ivo_david_michelle 10:e7d1801e966a 17 double q; // omega y
ivo_david_michelle 10:e7d1801e966a 18 double r; // omega z
ivo_david_michelle 6:6f3ffd97d808 19 };
ivo_david_michelle 7:f3f94eadc5b5 20
ivo_david_michelle 32:e12b01c94b4a 21 struct filter {
ivo_david_michelle 32:e12b01c94b4a 22 double phi[FILTER_SIZE]; // roll
ivo_david_michelle 32:e12b01c94b4a 23 double theta[FILTER_SIZE]; // pitch
ivo_david_michelle 32:e12b01c94b4a 24 double psi[FILTER_SIZE]; // yaw
ivo_david_michelle 32:e12b01c94b4a 25 double p[FILTER_SIZE]; // omega x
ivo_david_michelle 32:e12b01c94b4a 26 double q[FILTER_SIZE]; // omega y
ivo_david_michelle 32:e12b01c94b4a 27 double r[FILTER_SIZE]; // omega z
ivo_david_michelle 32:e12b01c94b4a 28 };
ivo_david_michelle 32:e12b01c94b4a 29
ivo_david_michelle 10:e7d1801e966a 30 struct controlInput {
ivo_david_michelle 10:e7d1801e966a 31 double f;
ivo_david_michelle 10:e7d1801e966a 32 double mx;
ivo_david_michelle 10:e7d1801e966a 33 double my;
ivo_david_michelle 10:e7d1801e966a 34 double mz;
ivo_david_michelle 7:f3f94eadc5b5 35 };
ivo_david_michelle 7:f3f94eadc5b5 36
ivo_david_michelle 14:64b06476d943 37 typedef struct {
ivo_david_michelle 24:e220fbb70ded 38 double gyro_x;
ivo_david_michelle 24:e220fbb70ded 39 double gyro_y;
ivo_david_michelle 24:e220fbb70ded 40 double gyro_z;
ivo_david_michelle 24:e220fbb70ded 41 double roll;
ivo_david_michelle 24:e220fbb70ded 42 double pitch;
ivo_david_michelle 24:e220fbb70ded 43 double heading;
ivo_david_michelle 14:64b06476d943 44 } offset;
ivo_david_michelle 7:f3f94eadc5b5 45
ivo_david_michelle 11:5c54826d23a7 46 struct motors {
ivo_david_michelle 11:5c54826d23a7 47 double m1;
ivo_david_michelle 11:5c54826d23a7 48 double m2;
ivo_david_michelle 11:5c54826d23a7 49 double m3;
ivo_david_michelle 11:5c54826d23a7 50 double m4;
ivo_david_michelle 13:291ba30c7806 51 };
ivo_david_michelle 10:e7d1801e966a 52
ivo_david_michelle 10:e7d1801e966a 53
ivo_david_michelle 24:e220fbb70ded 54 class Quadcopter
ivo_david_michelle 24:e220fbb70ded 55 {
ivo_david_michelle 6:6f3ffd97d808 56 private:
ivo_david_michelle 14:64b06476d943 57 Serial *pc_; // use for printing
ivo_david_michelle 24:e220fbb70ded 58
ivo_david_michelle 14:64b06476d943 59 // allow for remote communication
ivo_david_michelle 14:64b06476d943 60 MRF24J40 *mrf_;
ivo_david_michelle 14:64b06476d943 61 int rcIterations_;
ivo_david_michelle 14:64b06476d943 62 int rcLength_;
ivo_david_michelle 14:64b06476d943 63 Timer rcTimer_;
ivo_david_michelle 14:64b06476d943 64
ivo_david_michelle 32:e12b01c94b4a 65 filter filters_;
ivo_david_michelle 8:326e7009ce0c 66 state state_;
ivo_david_michelle 10:e7d1801e966a 67 state desiredState_;
ivo_david_michelle 29:ae765492fa8b 68 double F_des_; // desired thrust force (excluding weight compensation)
ivo_david_michelle 10:e7d1801e966a 69 controlInput controlInput_;
ivo_david_michelle 11:5c54826d23a7 70 motors motorPwm_;
ivo_david_michelle 24:e220fbb70ded 71
ivo_david_michelle 13:291ba30c7806 72 // declarations for IMU use
ivo_david_michelle 10:e7d1801e966a 73 Adafruit_9DOF dof_;
ivo_david_michelle 10:e7d1801e966a 74 Adafruit_LSM303_Accel_Unified accel_;
ivo_david_michelle 10:e7d1801e966a 75 Adafruit_LSM303_Mag_Unified mag_;
ivo_david_michelle 10:e7d1801e966a 76 Adafruit_L3GD20_Unified gyro_;
ivo_david_michelle 10:e7d1801e966a 77 sensors_event_t accel_event_;
ivo_david_michelle 9:f1bd96708a21 78 sensors_event_t mag_event_;
ivo_david_michelle 9:f1bd96708a21 79 sensors_event_t gyro_event_;
ivo_david_michelle 9:f1bd96708a21 80 sensors_vec_t orientation_;
ivo_david_michelle 24:e220fbb70ded 81 offset* initial_offsets_;
ivo_david_michelle 9:f1bd96708a21 82
ivo_david_michelle 10:e7d1801e966a 83 double g_; // gravity [m/s^2]
ivo_david_michelle 10:e7d1801e966a 84 double m_; // mass [kg]
ivo_david_michelle 20:efa15ed008b4 85 double l_; // arm lenght [m]
ivo_david_michelle 20:efa15ed008b4 86 double gamma_; // relation between moment coefficient and force coefficient.
ivo_david_michelle 24:e220fbb70ded 87
ivo_david_michelle 10:e7d1801e966a 88 // proportional attitude control gains
ivo_david_michelle 21:336faf452989 89 double kp_f_; // gain converting radio input to thrust.
ivo_david_michelle 10:e7d1801e966a 90 double kp_phi_;
ivo_david_michelle 10:e7d1801e966a 91 double kp_theta_;
ivo_david_michelle 10:e7d1801e966a 92 double kp_psi_;
ivo_david_michelle 10:e7d1801e966a 93
ivo_david_michelle 10:e7d1801e966a 94 // derivative attitude control gains
ivo_david_michelle 10:e7d1801e966a 95 double kd_phi_;
ivo_david_michelle 10:e7d1801e966a 96 double kd_theta_;
ivo_david_michelle 10:e7d1801e966a 97 double kd_psi_;
ivo_david_michelle 27:11116aa69f32 98
ivo_david_michelle 31:d473eacfc271 99 double ki_phi_;
ivo_david_michelle 31:d473eacfc271 100 double ki_theta_;
ivo_david_michelle 31:d473eacfc271 101 //double ki_psi_;
ivo_david_michelle 31:d473eacfc271 102 double i_e_phi_;
ivo_david_michelle 31:d473eacfc271 103 double i_e_theta_;
ivo_david_michelle 31:d473eacfc271 104 float prev_time_;
ivo_david_michelle 31:d473eacfc271 105
ivo_david_michelle 31:d473eacfc271 106
ivo_david_michelle 27:11116aa69f32 107 double zeroVelPwm;
ivo_david_michelle 27:11116aa69f32 108 double maxPwm;
ivo_david_michelle 31:d473eacfc271 109 double max_integral_phi_;
ivo_david_michelle 31:d473eacfc271 110 double max_integral_theta_;
ivo_david_michelle 31:d473eacfc271 111
ivo_david_michelle 27:11116aa69f32 112
ivo_david_michelle 14:64b06476d943 113 public:
ivo_david_michelle 15:90e07946186f 114 Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr); // constructor
ivo_david_michelle 7:f3f94eadc5b5 115
ivo_david_michelle 31:d473eacfc271 116 void controller(float time);
ivo_david_michelle 8:326e7009ce0c 117 void readSensorValues();
ivo_david_michelle 14:64b06476d943 118 void readRc();
ivo_david_michelle 14:64b06476d943 119
ivo_david_michelle 11:5c54826d23a7 120 motors getPwm();
ivo_david_michelle 24:e220fbb70ded 121 state getState();
ivo_david_michelle 24:e220fbb70ded 122 Adafruit_LSM303_Accel_Unified getAccel();
ivo_david_michelle 24:e220fbb70ded 123 Adafruit_LSM303_Mag_Unified getMag();
ivo_david_michelle 24:e220fbb70ded 124 Adafruit_L3GD20_Unified getGyro();
ivo_david_michelle 24:e220fbb70ded 125 Adafruit_9DOF getIMU();
ivo_david_michelle 24:e220fbb70ded 126 offset* getOffset();
ivo_david_michelle 27:11116aa69f32 127 double getForce();
ivo_david_michelle 6:6f3ffd97d808 128 };
ivo_david_michelle 7:f3f94eadc5b5 129
ivo_david_michelle 6:6f3ffd97d808 130 #endif