ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 10:e7d1801e966a
- Parent:
- 9:f1bd96708a21
- Child:
- 11:5c54826d23a7
diff -r f1bd96708a21 -r e7d1801e966a quadcopter.cpp --- a/quadcopter.cpp Sat Apr 02 14:54:46 2016 +0000 +++ b/quadcopter.cpp Sat Apr 02 17:41:37 2016 +0000 @@ -4,65 +4,90 @@ #include "Serial_base.h" -// Date constructor +// constructor Quadcopter::Quadcopter() { - setState(0.0,0.0,0.0); + m_=1; + g_=9.81; + // proportional attitude control gains + kp_phi_ = 1; + kp_theta_ = 1; + kp_psi_ = 1; + + // derivative attitude control gains + kd_phi_ = 0; + kd_theta_ = 0; + kd_psi_ = 0; + + // desired values (will come from joystick) + F_des_ = 0; // desired thrust force (excluding weight compensation) dof_ = Adafruit_9DOF(); accel_ = Adafruit_LSM303_Accel_Unified(30301); mag_ = Adafruit_LSM303_Mag_Unified(30302); gyro_ = Adafruit_L3GD20_Unified(20); - // pc.printf("Entering control loop\n\r"); } - - void Quadcopter::initAllSensors() { - print("Initializing Sensors with class method"); - initSensors(accel_, mag_, gyro_,offsetAngRate_); // IMU } - void Quadcopter::readSensorValues() { accel_.getEvent(&accel_event_); if (dof_.accelGetOrientation(&accel_event_, &orientation_)) { - /* 'orientation' should have valid .roll and .pitch fields */ - //s_com->print(("Roll: ")); - //s_com->print(orientation.roll); - //s_com->print(("; ")); - //s_com->print(("Pitch: ")); - //s_com->print(orientation.pitch); - //s_com->print((";\t")); } /* Calculate the heading using the magnetometer */ mag_.getEvent(&mag_event_); if (dof_.magGetOrientation(SENSOR_AXIS_Z, &mag_event_, &orientation_)) { - /* 'orientation' should have valid .heading data now */ - //s_com->print(("Heading: ")); - //s_com->print(orientation.heading); - // s_com->print((";\r\n")); } + // measured values (will come from IMU/parameter class/Input to function later) + // angles + state_.phi = orientation_.roll; + state_.theta =orientation_.pitch; + state_.psi =orientation_.heading; + // angular velocities in body coordinate system + state_.p = gyro_event_.gyro.x; + state_.q = gyro_event_.gyro.y; + state_.r = gyro_event_.gyro.z; +} +// Date member function +void Quadcopter::setState(state *source, state *goal) +{ + goal->phi = source->phi; + goal->theta = source->theta; + goal->psi = source->psi; + goal->p = source->p; + goal->q = source->q; + goal->r = source->r; } - -// Date member function -void Quadcopter::setState(double phi, double theta, double psi) +void Quadcopter::print(char * myString) { - state_.phi = phi; - state_.theta = theta; - state_.psi = psi; + pcPntr_->printf(myString); + pcPntr_->printf("\n\r"); } -void Quadcopter::print(char * myString){ - - (*pcPntr_).printf(myString); - (*pcPntr_).printf("\n\r"); +void Quadcopter::controller() +{ + + // compute desired angles (in the case we decide not to set + // the angles, but for instance the velocity with the Joystick - } \ No newline at end of file + // PD controller + controlInput_.f = m_*g_ + F_des_; + controlInput_.mx = kp_phi_*(desiredState_.phi-state_.phi)+kd_phi_*(desiredState_.p-state_.p); + controlInput_.my = kp_theta_*(desiredState_.theta-state_.theta)+kd_theta_*(desiredState_.q-state_.q); + controlInput_.mz = kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r); + print("Calculated Control"); + + //print("F: %f M_x: %f M_y: %f M_z: %f\n\r", controlInput_.f, controlInput_.mz, controlInput_.my, controlInput_.mz); + // pc.printf("F: %f\n\r", F); + + + +} \ No newline at end of file