David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

quadcopter.cpp@23:04338a5ef404, 2016-04-12 (annotated)

Committer:
ivo_david_michelle
Date:
Tue Apr 12 22:54:16 2016 +0000
Revision:
23:04338a5ef404
Parent:
22:92401a4fec13
Child:
24:e220fbb70ded
reading data from imu again

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivo_david_michelle 6:6f3ffd97d808 1 #include "quadcopter.h"
ivo_david_michelle 9:f1bd96708a21 2 #include "sensor.h"
ivo_david_michelle 14:64b06476d943 3 #include "receiver.h"
ivo_david_michelle 14:64b06476d943 4 #include <string>
ivo_david_michelle 23:04338a5ef404 5 #include <math.h>
ivo_david_michelle 23:04338a5ef404 6
ivo_david_michelle 23:04338a5ef404 7 #ifndef M_PI
ivo_david_michelle 23:04338a5ef404 8 #define M_PI 3.14159265358979323846
ivo_david_michelle 23:04338a5ef404 9 #endif
ivo_david_michelle 9:f1bd96708a21 10
ivo_david_michelle 14:64b06476d943 11 //#include "mbed.h"
ivo_david_michelle 9:f1bd96708a21 12
ivo_david_michelle 10:e7d1801e966a 13 // constructor
ivo_david_michelle 15:90e07946186f 14 Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr)
ivo_david_michelle 14:64b06476d943 15 {
ivo_david_michelle 14:64b06476d943 16 pc_= pcPntr; // enable printing
ivo_david_michelle 23:04338a5ef404 17 initSensors(accel_, mag_, gyro_, offsetAngRate_); // IMU
ivo_david_michelle 14:64b06476d943 18
ivo_david_michelle 23:04338a5ef404 19 m_= 1;
ivo_david_michelle 23:04338a5ef404 20 g_= 9.81;
ivo_david_michelle 23:04338a5ef404 21 l_= 0.25;
ivo_david_michelle 23:04338a5ef404 22 gamma_= 1;
ivo_david_michelle 20:efa15ed008b4 23
ivo_david_michelle 20:efa15ed008b4 24
ivo_david_michelle 10:e7d1801e966a 25 // proportional attitude control gains
ivo_david_michelle 23:04338a5ef404 26 // TODO change gains so that joystick deflection never produces pwm duty cycle >10%.
ivo_david_michelle 23:04338a5ef404 27
ivo_david_michelle 21:336faf452989 28 kp_f_ =2.5;
ivo_david_michelle 21:336faf452989 29 kp_phi_ = 0.2;
ivo_david_michelle 21:336faf452989 30 kp_theta_ = 0.2;
ivo_david_michelle 21:336faf452989 31 kp_psi_ = 0.2;
ivo_david_michelle 10:e7d1801e966a 32
ivo_david_michelle 10:e7d1801e966a 33 // derivative attitude control gains
ivo_david_michelle 10:e7d1801e966a 34 kd_phi_ = 0;
ivo_david_michelle 10:e7d1801e966a 35 kd_theta_ = 0;
ivo_david_michelle 10:e7d1801e966a 36 kd_psi_ = 0;
ivo_david_michelle 10:e7d1801e966a 37
ivo_david_michelle 10:e7d1801e966a 38 // desired values (will come from joystick)
ivo_david_michelle 10:e7d1801e966a 39 F_des_ = 0; // desired thrust force (excluding weight compensation)
ivo_david_michelle 9:f1bd96708a21 40
ivo_david_michelle 9:f1bd96708a21 41 dof_ = Adafruit_9DOF();
ivo_david_michelle 9:f1bd96708a21 42 accel_ = Adafruit_LSM303_Accel_Unified(30301);
ivo_david_michelle 9:f1bd96708a21 43 mag_ = Adafruit_LSM303_Mag_Unified(30302);
ivo_david_michelle 9:f1bd96708a21 44 gyro_ = Adafruit_L3GD20_Unified(20);
ivo_david_michelle 14:64b06476d943 45 //motor1_(p21);
ivo_david_michelle 14:64b06476d943 46
ivo_david_michelle 14:64b06476d943 47 // initSensors(accel_, mag_, gyro_, offsetAngRate_); // IMU
ivo_david_michelle 14:64b06476d943 48
ivo_david_michelle 14:64b06476d943 49 // prepare for communication with remote control
ivo_david_michelle 16:2be2aab63198 50 rcTimer_.start();
ivo_david_michelle 15:90e07946186f 51 mrf_ = mrfPntr; // RF tranceiver to link with handheld.
ivo_david_michelle 15:90e07946186f 52 rcLength_ = 250;
ivo_david_michelle 16:2be2aab63198 53 mrf_->SetChannel(3); //Set the Channel. 0 is default, 15 is max
ivo_david_michelle 17:96d0c72e413e 54 thrust = 0.5;
ivo_david_michelle 17:96d0c72e413e 55 yaw = 0.5;
ivo_david_michelle 17:96d0c72e413e 56 pitch = 0.5;
ivo_david_michelle 17:96d0c72e413e 57 roll = 0.5;
ivo_david_michelle 17:96d0c72e413e 58 id = -1;
ivo_david_michelle 9:f1bd96708a21 59 }
ivo_david_michelle 9:f1bd96708a21 60
ivo_david_michelle 9:f1bd96708a21 61
ivo_david_michelle 9:f1bd96708a21 62 void Quadcopter::readSensorValues()
ivo_david_michelle 9:f1bd96708a21 63 {
ivo_david_michelle 9:f1bd96708a21 64 accel_.getEvent(&accel_event_);
ivo_david_michelle 9:f1bd96708a21 65 if (dof_.accelGetOrientation(&accel_event_, &orientation_)) {
ivo_david_michelle 9:f1bd96708a21 66 }
ivo_david_michelle 9:f1bd96708a21 67 /* Calculate the heading using the magnetometer */
ivo_david_michelle 9:f1bd96708a21 68 mag_.getEvent(&mag_event_);
ivo_david_michelle 9:f1bd96708a21 69 if (dof_.magGetOrientation(SENSOR_AXIS_Z, &mag_event_, &orientation_)) {
ivo_david_michelle 9:f1bd96708a21 70 }
ivo_david_michelle 9:f1bd96708a21 71
ivo_david_michelle 13:291ba30c7806 72 gyro_.getEvent(&gyro_event_);
ivo_david_michelle 11:5c54826d23a7 73
ivo_david_michelle 13:291ba30c7806 74 gyro_event_.gyro.x -= offsetAngRate_.x;
ivo_david_michelle 13:291ba30c7806 75 gyro_event_.gyro.y -= offsetAngRate_.y;
ivo_david_michelle 13:291ba30c7806 76 gyro_event_.gyro.z -= offsetAngRate_.z;
ivo_david_michelle 13:291ba30c7806 77
ivo_david_michelle 13:291ba30c7806 78
ivo_david_michelle 10:e7d1801e966a 79 // measured values (will come from IMU/parameter class/Input to function later)
ivo_david_michelle 10:e7d1801e966a 80 // angles
ivo_david_michelle 10:e7d1801e966a 81 state_.phi = orientation_.roll;
ivo_david_michelle 10:e7d1801e966a 82 state_.theta =orientation_.pitch;
ivo_david_michelle 10:e7d1801e966a 83 state_.psi =orientation_.heading;
ivo_david_michelle 10:e7d1801e966a 84 // angular velocities in body coordinate system
ivo_david_michelle 10:e7d1801e966a 85 state_.p = gyro_event_.gyro.x;
ivo_david_michelle 10:e7d1801e966a 86 state_.q = gyro_event_.gyro.y;
ivo_david_michelle 10:e7d1801e966a 87 state_.r = gyro_event_.gyro.z;
ivo_david_michelle 23:04338a5ef404 88
ivo_david_michelle 23:04338a5ef404 89 // TODO print values to check what they are
ivo_david_michelle 23:04338a5ef404 90 // TODO convert to Radians (*pi/180)
ivo_david_michelle 23:04338a5ef404 91
ivo_david_michelle 23:04338a5ef404 92 pc_->printf("Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f\r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r);
ivo_david_michelle 23:04338a5ef404 93 state_.phi = orientation_.roll * M_PI / 180;
ivo_david_michelle 23:04338a5ef404 94 state_.theta =orientation_.pitch * M_PI / 180;
ivo_david_michelle 23:04338a5ef404 95 state_.psi =orientation_.heading * M_PI / 180;
ivo_david_michelle 23:04338a5ef404 96 //pc_->printf("Converted Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f\r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r);
ivo_david_michelle 10:e7d1801e966a 97 }
ivo_david_michelle 9:f1bd96708a21 98
ivo_david_michelle 10:e7d1801e966a 99 // Date member function
ivo_david_michelle 10:e7d1801e966a 100 void Quadcopter::setState(state *source, state *goal)
ivo_david_michelle 10:e7d1801e966a 101 {
ivo_david_michelle 10:e7d1801e966a 102 goal->phi = source->phi;
ivo_david_michelle 10:e7d1801e966a 103 goal->theta = source->theta;
ivo_david_michelle 10:e7d1801e966a 104 goal->psi = source->psi;
ivo_david_michelle 10:e7d1801e966a 105 goal->p = source->p;
ivo_david_michelle 10:e7d1801e966a 106 goal->q = source->q;
ivo_david_michelle 10:e7d1801e966a 107 goal->r = source->r;
ivo_david_michelle 9:f1bd96708a21 108
ivo_david_michelle 9:f1bd96708a21 109 }
ivo_david_michelle 9:f1bd96708a21 110
ivo_david_michelle 9:f1bd96708a21 111
ivo_david_michelle 10:e7d1801e966a 112 void Quadcopter::controller()
ivo_david_michelle 10:e7d1801e966a 113 {
ivo_david_michelle 10:e7d1801e966a 114
ivo_david_michelle 10:e7d1801e966a 115 // compute desired angles (in the case we decide not to set
ivo_david_michelle 10:e7d1801e966a 116 // the angles, but for instance the velocity with the Joystick
ivo_david_michelle 9:f1bd96708a21 117
ivo_david_michelle 10:e7d1801e966a 118 // PD controller
ivo_david_michelle 21:336faf452989 119 controlInput_.f = kp_f_*F_des_;//m_*g_ + F_des_;
ivo_david_michelle 10:e7d1801e966a 120 controlInput_.mx = kp_phi_*(desiredState_.phi-state_.phi)+kd_phi_*(desiredState_.p-state_.p);
ivo_david_michelle 10:e7d1801e966a 121 controlInput_.my = kp_theta_*(desiredState_.theta-state_.theta)+kd_theta_*(desiredState_.q-state_.q);
ivo_david_michelle 10:e7d1801e966a 122 controlInput_.mz = kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r);
ivo_david_michelle 14:64b06476d943 123 //print("Calculated Control");
ivo_david_michelle 10:e7d1801e966a 124
ivo_david_michelle 10:e7d1801e966a 125 //print("F: %f M_x: %f M_y: %f M_z: %f\n\r", controlInput_.f, controlInput_.mz, controlInput_.my, controlInput_.mz);
ivo_david_michelle 14:64b06476d943 126 // pc_->printf("F: %f\n\r", F);
ivo_david_michelle 20:efa15ed008b4 127
ivo_david_michelle 20:efa15ed008b4 128 // set pwm values
ivo_david_michelle 20:efa15ed008b4 129 // make code faster by precomputing all the components that are used multiple times and hardcode 0.25/gamma...
ivo_david_michelle 22:92401a4fec13 130 double zeroVeloPwm=0.1;
ivo_david_michelle 22:92401a4fec13 131 motorPwm_.m1=zeroVeloPwm+ 0.25*controlInput_.f-0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
ivo_david_michelle 22:92401a4fec13 132 motorPwm_.m2=zeroVeloPwm +0.25*controlInput_.f-0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
ivo_david_michelle 22:92401a4fec13 133 motorPwm_.m3=zeroVeloPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
ivo_david_michelle 22:92401a4fec13 134 motorPwm_.m4=zeroVeloPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
ivo_david_michelle 20:efa15ed008b4 135
ivo_david_michelle 13:291ba30c7806 136 }
ivo_david_michelle 10:e7d1801e966a 137
ivo_david_michelle 12:422963993df5 138 motors Quadcopter::getPwm()
ivo_david_michelle 11:5c54826d23a7 139 {
ivo_david_michelle 22:92401a4fec13 140
ivo_david_michelle 22:92401a4fec13 141 return motorPwm_;
ivo_david_michelle 22:92401a4fec13 142 }
ivo_david_michelle 20:efa15ed008b4 143
ivo_david_michelle 22:92401a4fec13 144 state Quadcopter::getState()
ivo_david_michelle 22:92401a4fec13 145 {
ivo_david_michelle 22:92401a4fec13 146
ivo_david_michelle 22:92401a4fec13 147 return state_;
ivo_david_michelle 11:5c54826d23a7 148 }
ivo_david_michelle 14:64b06476d943 149
ivo_david_michelle 14:64b06476d943 150
ivo_david_michelle 16:2be2aab63198 151 void Quadcopter::readRc() {
ivo_david_michelle 14:64b06476d943 152 uint8_t zero = 0;
ivo_david_michelle 14:64b06476d943 153 uint8_t *rssi = &zero;
ivo_david_michelle 18:a00d6b065c6b 154
ivo_david_michelle 14:64b06476d943 155
ivo_david_michelle 14:64b06476d943 156 uint8_t receive = 0;
ivo_david_michelle 14:64b06476d943 157
ivo_david_michelle 14:64b06476d943 158 char rxBuffer[rcLength_];
ivo_david_michelle 14:64b06476d943 159
ivo_david_michelle 18:a00d6b065c6b 160
ivo_david_michelle 16:2be2aab63198 161 receive = rf_receive_rssi(*mrf_, rxBuffer, rssi, rcLength_ + 1);
ivo_david_michelle 16:2be2aab63198 162 if (receive > 0) {
ivo_david_michelle 17:96d0c72e413e 163 sscanf(rxBuffer, "%lld,%f,%f,%f,%f", &id, &thrust, &yaw, &pitch, &roll);
ivo_david_michelle 16:2be2aab63198 164 } else {
ivo_david_michelle 16:2be2aab63198 165 pc_->printf("Receive failure\r\n");
ivo_david_michelle 16:2be2aab63198 166 }
ivo_david_michelle 23:04338a5ef404 167
ivo_david_michelle 23:04338a5ef404 168 // TODO convert to radians (starting point: full joystick deflection = +-10° (converted to radian).
ivo_david_michelle 23:04338a5ef404 169
ivo_david_michelle 18:a00d6b065c6b 170 desiredState_.phi=roll-0.5;
ivo_david_michelle 18:a00d6b065c6b 171 desiredState_.theta=pitch-0.5;
ivo_david_michelle 18:a00d6b065c6b 172 desiredState_.psi=yaw-0.5;
ivo_david_michelle 18:a00d6b065c6b 173 F_des_=thrust-0.5;
ivo_david_michelle 23:04338a5ef404 174
ivo_david_michelle 23:04338a5ef404 175
ivo_david_michelle 14:64b06476d943 176 }
ivo_david_michelle 14:64b06476d943 177