ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 23:04338a5ef404
- Parent:
- 22:92401a4fec13
- Child:
- 24:e220fbb70ded
diff -r 92401a4fec13 -r 04338a5ef404 quadcopter.cpp --- a/quadcopter.cpp Mon Apr 11 16:41:56 2016 +0000 +++ b/quadcopter.cpp Tue Apr 12 22:54:16 2016 +0000 @@ -2,6 +2,11 @@ #include "sensor.h" #include "receiver.h" #include <string> +#include <math.h> + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif //#include "mbed.h" @@ -9,15 +14,17 @@ Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr) { pc_= pcPntr; // enable printing - // initSensors(accel_, mag_, gyro_, offsetAngRate_); // IMU + initSensors(accel_, mag_, gyro_, offsetAngRate_); // IMU - m_=1; - g_=9.81; - l_=0.25; - gamma_=1; + m_= 1; + g_= 9.81; + l_= 0.25; + gamma_= 1; // proportional attitude control gains + // TODO change gains so that joystick deflection never produces pwm duty cycle >10%. + kp_f_ =2.5; kp_phi_ = 0.2; kp_theta_ = 0.2; @@ -78,6 +85,15 @@ state_.p = gyro_event_.gyro.x; state_.q = gyro_event_.gyro.y; state_.r = gyro_event_.gyro.z; + + // TODO print values to check what they are + // TODO convert to Radians (*pi/180) + + 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); + state_.phi = orientation_.roll * M_PI / 180; + state_.theta =orientation_.pitch * M_PI / 180; + state_.psi =orientation_.heading * M_PI / 180; + //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); } // Date member function @@ -148,9 +164,14 @@ } else { pc_->printf("Receive failure\r\n"); } + + // TODO convert to radians (starting point: full joystick deflection = +-10° (converted to radian). + desiredState_.phi=roll-0.5; desiredState_.theta=pitch-0.5; desiredState_.psi=yaw-0.5; F_des_=thrust-0.5; + + }