ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 32:e12b01c94b4a
- Parent:
- 31:d473eacfc271
- Child:
- 33:244dea7a4e81
diff -r d473eacfc271 -r e12b01c94b4a quadcopter.cpp --- a/quadcopter.cpp Thu Apr 21 20:57:19 2016 +0000 +++ b/quadcopter.cpp Fri Apr 22 00:52:32 2016 +0000 @@ -18,31 +18,32 @@ 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; + // 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.16 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; + kp_theta_ = 0.12 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; kp_psi_ = 0; - // kp_phi_ = 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_phi_ = 0.025 * (maxPwm - zeroVelPwm) * 2 / M_PI; + kd_theta_ = 0.025 * (maxPwm - zeroVelPwm) * 2 / M_PI; kd_psi_ = 0.1; - // kd_phi_ = 0; + // 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; +// incresae ki_phi + ki_phi_ = 0.1 * (maxPwm - zeroVelPwm)/(2*M_PI/4); // full control signal after 2s at pi/4 error + ki_theta_ = 0 * (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)); + max_integral_theta_ = 0.05/(ki_theta_ * (0.5/l_ + 0.25)); // desired values (will come from joystick) @@ -82,35 +83,63 @@ orientation_.pitch -= initial_offsets_->pitch; orientation_.heading -= initial_offsets_->heading; + static int current_filter = 0; + filters_.p[current_filter] = gyro_event_.gyro.x * M_PI / 180; + filters_.q[current_filter] = gyro_event_.gyro.y * M_PI / 180; + filters_.r[current_filter] = gyro_event_.gyro.z * M_PI / 180; + filters_.phi[current_filter] = orientation_.roll * M_PI / 180; + filters_.theta[current_filter] = -orientation_.pitch * M_PI / 180; + filters_.psi[current_filter] = orientation_.heading * M_PI / 180; + + current_filter = (current_filter + 1) % FILTER_SIZE; + + double p_sum = 0; + double q_sum = 0; + double r_sum = 0; + double phi_sum = 0; + double theta_sum = 0; + double psi_sum = 0; + for (int i = 0; i < FILTER_SIZE; i++) { + p_sum += filters_.p[i]; + q_sum += filters_.q[i]; + r_sum += filters_.r[i]; + phi_sum += filters_.phi[i]; + theta_sum += filters_.theta[i]; + psi_sum += filters_.psi[i]; + } + // angular velocities in body coordinate system - state_.p = gyro_event_.gyro.x * M_PI / 180; - state_.q = gyro_event_.gyro.y * M_PI / 180; - state_.r = gyro_event_.gyro.z * M_PI / 180; + state_.p = p_sum / FILTER_SIZE; + state_.q = q_sum / FILTER_SIZE; + state_.r = r_sum / FILTER_SIZE; - state_.phi = orientation_.roll * M_PI / 180; - state_.theta = -orientation_.pitch * M_PI / 180; - state_.psi = orientation_.heading * M_PI / 180; + state_.phi = phi_sum / FILTER_SIZE; + state_.theta = theta_sum / FILTER_SIZE; + state_.psi = psi_sum / FILTER_SIZE; + //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(float time) { - + if (prev_time_ == 0) { - prev_time_ = time; + 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; + + 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_); + i_e_phi_ = max(-max_integral_phi_, i_e_phi_); + i_e_theta_ = max(-max_integral_theta_, i_e_theta_); controlInput_.f = kp_f_ * F_des_;//m_*g_ + F_des_; controlInput_.mx = kp_phi_ * e_phi + kd_phi_ * (desiredState_.p - state_.p) + ki_phi_ * i_e_phi_; @@ -129,12 +158,15 @@ // 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(0.17, motorPwm_.m1); + motorPwm_.m1 = min(maxPwm, motorPwm_.m1); motorPwm_.m2 = min(maxPwm, motorPwm_.m2); motorPwm_.m3 = min(maxPwm, motorPwm_.m3); motorPwm_.m4 = min(maxPwm, motorPwm_.m4); - - prev_time_ = time; + + prev_time_ = time; + + //pc_->printf("m1: %f\tm2: %f\tm3: %f\tm4: %f\r\n", motorPwm_.m1, motorPwm_.m2, motorPwm_.m3, motorPwm_.m4); + } motors Quadcopter::getPwm()