Diff: quadcopter.cpp

Revision:

30:4820042e67b5

Parent:

29:ae765492fa8b

Child:

31:d473eacfc271

--- a/quadcopter.cpp	Fri Apr 15 19:33:14 2016 +0000
+++ b/quadcopter.cpp	Wed Apr 20 02:29:37 2016 +0000
@@ -15,18 +15,20 @@
     l_= 0.25;
     gamma_= 1;
 
-    zeroVelPwm=0.1;
+    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_   = (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;
-    kp_theta_ = (maxPwm - zeroVelPwm) * l_ / 0.5 * 4 / M_PI;;
+    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;
 
     // derivative attitude control gains
-    kd_phi_   = 0;
-    kd_theta_ = 0;
+    kd_phi_   = 0.3 * (maxPwm - zeroVelPwm) * 2 / M_PI;
+    kd_theta_ = 0.3 * (maxPwm - zeroVelPwm) * 2 / M_PI;
     kd_psi_   = 0.1;
 
     // desired values (will come from joystick)
@@ -67,9 +69,9 @@
     orientation_.heading -= initial_offsets_->heading;
 
     // angular velocities in body coordinate system
-    state_.p = gyro_event_.gyro.x;
-    state_.q = gyro_event_.gyro.y;
-    state_.r = gyro_event_.gyro.z;
+    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_.phi = orientation_.roll * M_PI / 180;
     state_.theta = -orientation_.pitch * M_PI / 180;
@@ -96,6 +98,7 @@
     motorPwm_.m4 = zeroVelPwm + 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);
@@ -168,7 +171,7 @@
     desiredState_.phi = -((roll - 0.5) * 80) * M_PI / 180; // minus, because joystick to right should result in positive moment
     desiredState_.theta = ((pitch - 0.5) * 80) * M_PI / 180;
     desiredState_.r = yaw-0.5; // number between 0 and 1 //((yaw - 0.5) * 80) * M_PI / 180;
-    F_des_ = thrust-0.5; // number between 0 and 1  //((thrust - 0.5) * 80) * M_PI / 180;
+    F_des_ = thrust-0.5; // number between 0 and 1 -> number between -0.5 and 0.5.  //((thrust - 0.5) * 80) * M_PI / 180;
 
     // print id with thrust, yaw, pitch, and roll
     //pc_->printf("%lld: thrust: %f, yaw: %f, pitch: %f, roll: %f\r\n", id, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi);