ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 42:d09dec5bb184
- Parent:
- 41:d103f9aa44f0
- Child:
- 43:0e98d5488bf2
diff -r d103f9aa44f0 -r d09dec5bb184 quadcopter.cpp --- a/quadcopter.cpp Wed May 04 18:46:06 2016 +0000 +++ b/quadcopter.cpp Wed May 04 21:35:05 2016 +0000 @@ -15,24 +15,22 @@ l_= 0.25; gamma_= 1; - zeroVelPwm = 0.11; - maxPwm = 0.15; desired_mutex = desired; // 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_ = 10 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; - kp_theta_ = 10 * (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI; + kp_f_ = (MAX_PWM - OFF_PWM) * 4 / 0.5; + kp_phi_ = 2.5 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI; + kp_theta_ = 2.5 * (MAX_PWM - OFF_PWM) * l_ / 0.5 * 4 / M_PI; kp_psi_ = 0; // derivative attitude control gains - kd_phi_ = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI; // 0.25 maybe a good - kd_theta_ = 0.0 * (maxPwm - zeroVelPwm) * 2 / M_PI; + kd_phi_ = 0.0 * (MAX_PWM - OFF_PWM) * 2 / M_PI; // 0.25 maybe a good + kd_theta_ = 0.0 * (MAX_PWM - OFF_PWM) * 2 / M_PI; kd_psi_ = 0.1; // incresae ki_phi - ki_phi_ = 0 * (maxPwm - zeroVelPwm)/(2*M_PI/4); // full control signal after 2s at pi/4 error - ki_theta_ = 0 * (maxPwm - zeroVelPwm)/(2*M_PI/4); + ki_phi_ = 0 * (MAX_PWM - OFF_PWM)/(2*M_PI/4); // full control signal after 2s at pi/4 error + ki_theta_ = 0 * (MAX_PWM - OFF_PWM)/(2*M_PI/4); i_e_phi_ = 0; i_e_theta_ = 0; @@ -91,9 +89,9 @@ accel_.getEvent(&accel_event_); // mag_.getEvent(&mag_event_); // detract offsets - accel_event.acceleration.x -= offset_acc_x; - accel_event.acceleration.y -= offset_acc_y; - accel_event.acceleration.z -= offset_acc_z; + accel_event_.acceleration.x -= offset_acc_x; + accel_event_.acceleration.y -= offset_acc_y; + accel_event_.acceleration.z -= offset_acc_z; dof_.accelGetOrientation(&accel_event_, &orientation_); gyro_.getEvent(&gyro_event_); @@ -183,18 +181,18 @@ prev_kalman_time = time; // RAW DATA - pc_->printf("%f %f %f %f %f %f %f \r\n", + /*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++; // 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); + 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() @@ -227,17 +225,22 @@ double yawMomentPerMotor = 0.25 / gamma_ * controlInput_.mz; double rollMomentPerMotor = 0.5 / l_ * controlInput_.mx; double pitchMomentPerMotor = 0.5 / l_ * controlInput_.my; - motorPwm_.m1 = zeroVelPwm + forcePerMotor - pitchMomentPerMotor - yawMomentPerMotor; - motorPwm_.m2 = zeroVelPwm + forcePerMotor + rollMomentPerMotor + yawMomentPerMotor; - motorPwm_.m3 = zeroVelPwm + forcePerMotor + pitchMomentPerMotor - yawMomentPerMotor; - motorPwm_.m4 = zeroVelPwm + forcePerMotor - rollMomentPerMotor + yawMomentPerMotor; + motorPwm_.m1 = MIN_PWM_1 + forcePerMotor - pitchMomentPerMotor - yawMomentPerMotor; + motorPwm_.m2 = MIN_PWM_2 + forcePerMotor + rollMomentPerMotor + yawMomentPerMotor; + motorPwm_.m3 = MIN_PWM_3 + forcePerMotor + pitchMomentPerMotor - yawMomentPerMotor; + motorPwm_.m4 = MIN_PWM_4 + forcePerMotor - rollMomentPerMotor + yawMomentPerMotor; // 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_.m2 = min(maxPwm, motorPwm_.m2); - motorPwm_.m3 = min(maxPwm, motorPwm_.m3); - motorPwm_.m4 = min(maxPwm, motorPwm_.m4); + motorPwm_.m1 = min(MAX_PWM, motorPwm_.m1); + motorPwm_.m2 = min(MAX_PWM, motorPwm_.m2); + motorPwm_.m3 = min(MAX_PWM, motorPwm_.m3); + motorPwm_.m4 = min(MAX_PWM, motorPwm_.m4); + + motorPwm_.m1 = max(MIN_PWM_1, motorPwm_.m1); + motorPwm_.m2 = max(MIN_PWM_2, motorPwm_.m2); + motorPwm_.m3 = max(MIN_PWM_3, motorPwm_.m3); + motorPwm_.m4 = max(MIN_PWM_4, motorPwm_.m4); prev_time_ = time;