Diff: quadcopter.cpp

Revision:

42:d09dec5bb184

Parent:

41:d103f9aa44f0

Child:

43:0e98d5488bf2

--- 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;