ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 31:d473eacfc271
- Parent:
- 30:4820042e67b5
- Child:
- 32:e12b01c94b4a
--- a/quadcopter.cpp Wed Apr 20 02:29:37 2016 +0000 +++ b/quadcopter.cpp Thu Apr 21 20:57:19 2016 +0000 @@ -15,21 +15,35 @@ l_= 0.25; gamma_= 1; - zeroVelPwm=0.11; - maxPwm=0.15; + zeroVelPwm = 0.11; + maxPwm = 0.15; // 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.2 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; kp_theta_ = 0.2 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; kp_psi_ = 0; - //kp_phi_ = 0; - //kp_theta_ = 0; + // kp_phi_ = 0; +// kp_theta_ = 0; // derivative attitude control gains kd_phi_ = 0.3 * (maxPwm - zeroVelPwm) * 2 / M_PI; kd_theta_ = 0.3 * (maxPwm - zeroVelPwm) * 2 / M_PI; kd_psi_ = 0.1; + // kd_phi_ = 0; + //kd_theta_ = 0; + + ki_phi_ = 0.5 * (maxPwm - zeroVelPwm)/(2*M_PI/4); // full control signal after 2s at pi/4 error + ki_theta_ = 0.5 * (maxPwm - zeroVelPwm)/(2*M_PI/4); + + i_e_phi_ = 0; + i_e_theta_ = 0; + prev_time_ = 0; + + + max_integral_phi_ = 0.05/(ki_phi_ * (0.5/l_ + 0.25)); // influence of integral part smaller than 0.05 + max_integral_theta_ = 0.05/(ki_theta_ * (0.5/l_ + 0.25)); + // desired values (will come from joystick) F_des_ = 0; // desired thrust force (excluding weight compensation) @@ -79,12 +93,28 @@ //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); } -void Quadcopter::controller() +void Quadcopter::controller(float time) { + + if (prev_time_ == 0) { + prev_time_ = time; + return; + } + + // PD controller + double e_phi = desiredState_.phi - state_.phi; + double e_theta = desiredState_.theta - state_.theta; + + float dt = time - prev_time_; + i_e_phi_ = i_e_phi_ + e_phi * dt; + i_e_theta_ = i_e_theta_ + e_theta * dt; + i_e_phi_ = min(max_integral_phi_, i_e_phi_); + i_e_theta_ = min(max_integral_theta_, i_e_theta_); + controlInput_.f = kp_f_ * F_des_;//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_.mx = kp_phi_ * e_phi + kd_phi_ * (desiredState_.p - state_.p) + ki_phi_ * i_e_phi_; + controlInput_.my = kp_theta_ * e_theta + kd_theta_ * (desiredState_.q - state_.q) + ki_theta_ * i_e_theta_; controlInput_.mz = kd_psi_ * desiredState_.r; // feedforward desired yaw rate. // kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r); // set pwm values @@ -99,10 +129,12 @@ // cut off at max PWM //pc_->printf("m1: %f\tm2: %f\tm3: %f\tm4: %f\r\n", motorPwm_.m1, motorPwm_.m2, motorPwm_.m3, motorPwm_.m4); - motorPwm_.m1 = min(maxPwm, motorPwm_.m1); + motorPwm_.m1 = min(0.17, motorPwm_.m1); motorPwm_.m2 = min(maxPwm, motorPwm_.m2); motorPwm_.m3 = min(maxPwm, motorPwm_.m3); motorPwm_.m4 = min(maxPwm, motorPwm_.m4); + + prev_time_ = time; } motors Quadcopter::getPwm()