Diff: quadcopter.cpp

Revision:

31:d473eacfc271

Parent:

30:4820042e67b5

Child:

32:e12b01c94b4a

diff -r 4820042e67b5 -r d473eacfc271 quadcopter.cpp
--- 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()