ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 47:ae478c380136
- Parent:
- 46:4bcf2e679f96
- Child:
- 48:0346cdff12a7
diff -r 4bcf2e679f96 -r ae478c380136 quadcopter.cpp --- a/quadcopter.cpp Thu May 05 15:58:49 2016 +0000 +++ b/quadcopter.cpp Thu May 05 20:47:55 2016 +0000 @@ -98,8 +98,8 @@ // compute orientation with correctet acc data dof_.accelGetOrientation(&accel_event_, &orientation_); // compute roll and pitch values - double raw_phi = orientation_.roll * M_PI / 180;; - double raw_theta = orientation_.pitch * M_PI / 180;; + double raw_phi = orientation_.roll * M_PI / 180; + double raw_theta = orientation_.pitch * M_PI / 180; // get gyro values gyro_.getEvent(&gyro_event_); @@ -123,10 +123,10 @@ // complementary filter parameters double alphaX = 0.0002;//0.002; - double alphaY = 0.0001; //0.002; + double alphaY = 0.0001; //maybe 0.0002; // different version of complementary filter //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; + //compAngleY = (1 - alphaY) * (com pAngleY + raw_q * dt) + alphaY * raw_theta; compAngleX = (1 - alphaX) * (compAngleX + (raw_p + 8.446703927263016e-04) * dt) + alphaX * raw_phi; // hardcoded value compensates drift with alpha == 0. compAngleY = (1 - alphaY) * (compAngleY + (raw_q - 0.008936763695439) * dt) + alphaY * raw_theta; @@ -151,7 +151,7 @@ // pc_->printf("%d\r\n", count); // } // count++; - //pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", prev_sensor_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,accel_event_.acceleration.x, accel_event_.acceleration.y, accel_event_.acceleration.z); + pc_->printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\r\n", prev_sensor_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,accel_event_.acceleration.x, accel_event_.acceleration.y, accel_event_.acceleration.z); } void Quadcopter::controller() @@ -180,7 +180,7 @@ controlInput_.f = kp_f_ * F_des_; 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); + controlInput_.mz = kd_psi_ * (desiredState_.r - state_.q); // feedforward desired yaw rate. // kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r); // set pwm values double forcePerMotor = 0.25 * controlInput_.f; @@ -294,8 +294,8 @@ // } // convert to radians, range is = +-40° or +-0.698132 radians - desiredState_.phi = 1 * (-(roll * 80) * M_PI / 180); // minus, because joystick to right should result in positive moment - desiredState_.theta = 1 * (pitch * 80) * M_PI / 180; + desiredState_.phi = 1 * (-(roll * 30) * M_PI / 180); // minus, because joystick to right should result in positive moment + desiredState_.theta = 1 * (pitch * 30) * M_PI / 180; desiredState_.r = 1 * yaw; // number between 0 and 1 //((yaw - 0.5) * 80) * M_PI / 180; F_des_ = thrust; // number between 0 and 1 -> number between -0.5 and 0.5