David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

quadcopter.h@31:d473eacfc271, 2016-04-21 (annotated)

Committer:
ivo_david_michelle
Date:
Thu Apr 21 20:57:19 2016 +0000
Revision:
31:d473eacfc271
Parent:
29:ae765492fa8b
Child:
32:e12b01c94b4a
implemented integral control

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