ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 40:09a59d5b7944
- Parent:
- 39:fff0a72633ee
- Child:
- 41:d103f9aa44f0
diff -r fff0a72633ee -r 09a59d5b7944 quadcopter.cpp --- a/quadcopter.cpp Sun May 01 22:13:01 2016 +0000 +++ b/quadcopter.cpp Wed May 04 17:28:48 2016 +0000 @@ -21,13 +21,13 @@ // control gains set s.t. 100% joystick results in 15% (actually: (maxPwm-zeroVelPwm+0.1)) duty cycle. kp_f_ = (maxPwm - zeroVelPwm) * 4 / 0.5; - kp_phi_ = 0.25 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; - kp_theta_ = 0.2 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; + kp_phi_ = 10 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; + kp_theta_ = 10 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; kp_psi_ = 0; // derivative attitude control gains - kd_phi_ = 0.05 * (maxPwm - zeroVelPwm) * 2 / M_PI; // 0.25 maybe a good - kd_theta_ = 0.05 * (maxPwm - zeroVelPwm) * 2 / M_PI; + kd_phi_ = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI; // 0.25 maybe a good + kd_theta_ = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI; kd_psi_ = 0.1; // incresae ki_phi @@ -83,7 +83,7 @@ accel_.getEvent(&accel_event_); // mag_.getEvent(&mag_event_); dof_.accelGetOrientation(&accel_event_, &orientation_); - // gyro_.getEvent(&gyro_event_); + gyro_.getEvent(&gyro_event_); gyro_event_.gyro.x -= initial_offsets_->gyro_x; gyro_event_.gyro.y -= initial_offsets_->gyro_y; @@ -148,21 +148,33 @@ state_.p = kalmanRoll.getRate() * M_PI / 180; state_.q = kalmanPitch.getRate() * M_PI / 180; - double alphaX = 0.7; - double alphaY = 0.7; - compAngleX = (1 - alphaX) * (compAngleX + raw_p * dt) + alphaX * raw_phi; // Calculate the angle using a Complimentary filter - compAngleY = (1 - alphaY) * (compAngleY + raw_q * dt) + alphaY * raw_theta; + double alphaX = 0.001; + double alphaY = 0.001; + //compAngleX = (1 - alphaX) * (compAngleX + raw_p * dt) + alphaX * raw_phi; // Calculate the angle using a Complimentary filter + //compAngleY = (1 - alphaY) * (compAngleY + raw_q * dt) + alphaY * raw_theta; + compAngleX = (1 - alphaX) * compAngleX + raw_p * dt + alphaX * raw_phi; // Calculate the angle using a Complimentary filter + compAngleY = (1 - alphaY) * compAngleY + raw_q * dt + alphaY * raw_theta; - //state_.phi = compAngleX; - //state_.theta = compAngleY; + + + state_.phi = compAngleX; + state_.theta = compAngleY; prev_kalman_time = time; + + /* RAW DATA + pc_->printf("%f %f %f %f %f %f %f \r\n", + prev_kalman_time, + accel_event_.acceleration.x, accel_event_.acceleration.y, accel_event_.acceleration.z, + gyro_event_.gyro.x, gyro_event_.gyro.y, gyro_event_.gyro.z); + */ // static int count = 0; // if (count % 100 == 0) { // pc_->printf("%d\r\n", count); // } // count++; - //pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", prev_kalman_time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi,state_.r, state_.p, state_.q, compAngleX, compAngleY, raw_phi, raw_theta); + // TODO: print HERE + // pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", prev_kalman_time, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi, state_.psi, state_.theta, state_.phi,state_.r, state_.p, state_.q, compAngleX, compAngleY, raw_phi, raw_theta); } void Quadcopter::controller()