Diff: quadcopter.cpp

Revision:

27:11116aa69f32

Parent:

26:7f50323c0c0d

Child:

28:61f7356325c3

diff -r 7f50323c0c0d -r 11116aa69f32 quadcopter.cpp
--- a/quadcopter.cpp	Thu Apr 14 19:58:42 2016 +0000
+++ b/quadcopter.cpp	Thu Apr 14 22:32:30 2016 +0000
@@ -12,29 +12,38 @@
 // constructor
 Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr)
 {
+
+
     pc_= pcPntr;  // enable printing
     //initSensors(accel_, mag_, gyro_, offsetAngRate_);  // IMUm_= 1;
     g_= 9.81;
     l_= 0.25;
     gamma_= 1;
 
+    zeroVelPwm=0.1;
+    maxPwm=0.15;
+
 
     // proportional attitude control gains
     // TODO change gains so that joystick deflection never produces pwm duty cycle >10%.
 
-    kp_f_ =2.5;
-    kp_phi_ = 0.2;
-    kp_theta_ = 0.2;
-    kp_psi_ = 0.2;
+
+    // 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_psi_ = 0;
 
     // derivative attitude control gains
     kd_phi_ = 0;
     kd_theta_ = 0;
-    kd_psi_ = 0;
+    kd_psi_ = 0.1;
 
     // desired values (will come from joystick)
     F_des_ = 0; // desired thrust force (excluding weight compensation)
 
+
+
     dof_ = Adafruit_9DOF();
     accel_ = Adafruit_LSM303_Accel_Unified(30301);
     mag_ = Adafruit_LSM303_Mag_Unified(30302);
@@ -48,11 +57,7 @@
     mrf_ = mrfPntr;  // RF tranceiver to link with handheld.
     rcLength_ = 250;
     mrf_->SetChannel(3);  //Set the Channel. 0 is default, 15 is max
-    thrust = 0.5;
-    yaw = 0.5;
-    pitch = 0.5;
-    roll = 0.5;
-    id = -1;
+
     initial_offsets_ = (offset*) malloc(sizeof(offset));
     initSensors(*this);  // IMU
 }
@@ -117,7 +122,7 @@
     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_.mz = kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r);
+    controlInput_.mz = kd_psi_*desiredState_.r; // feedforward desired yaw rate.  // kp_psi_*(desiredState_.psi-state_.psi)+kd_psi_*(desiredState_.r-state_.r);
     //print("Calculated Control");
 
     //pc_->printf("F: %f M_x: %f M_y: %f M_z: %f\n\r",  controlInput_.f, controlInput_.mz, controlInput_.my, controlInput_.mz);
@@ -125,11 +130,17 @@
 
     // set pwm values
     // make code faster by precomputing all the components that are used multiple times and hardcode 0.25/gamma...
-    double zeroVeloPwm=0.1;
-    motorPwm_.m1=zeroVeloPwm+ 0.25*controlInput_.f-0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
-    motorPwm_.m2=zeroVeloPwm +0.25*controlInput_.f-0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
-    motorPwm_.m3=zeroVeloPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
-    motorPwm_.m4=zeroVeloPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
+    motorPwm_.m1=zeroVelPwm + 0.25*controlInput_.f-0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
+    motorPwm_.m2=zeroVelPwm + 0.25*controlInput_.f-0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
+    motorPwm_.m3=zeroVelPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
+    motorPwm_.m4=zeroVelPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
+
+    motorPwm_.m1 = min(maxPwm, motorPwm_.m1);
+    motorPwm_.m2 = min(maxPwm, motorPwm_.m2);
+    motorPwm_.m3 = min(maxPwm, motorPwm_.m3);
+    motorPwm_.m4 = min(maxPwm, motorPwm_.m4);
+
+
 }
 
 motors Quadcopter::getPwm()
@@ -167,6 +178,12 @@
     return dof_;
 }
 
+double Quadcopter::getForce()
+{
+    return F_des_;
+}
+
+
 void Quadcopter::readRc()
 {
     uint8_t zero = 0;
@@ -176,6 +193,11 @@
 
     char rxBuffer[rcLength_];
 
+    float thrust;
+    float yaw;
+    float pitch;
+    float roll;
+    long long id;
 
     receive = rf_receive_rssi(*mrf_, rxBuffer, rssi, rcLength_ + 1);
     if (receive > 0) {
@@ -187,10 +209,10 @@
     // convert to radians, range is = +-40° or +-0.698132 radians
     desiredState_.phi = ((roll - 0.5) * 80) * M_PI / 180;
     desiredState_.theta = ((pitch - 0.5) * 80) * M_PI / 180;
-    desiredState_.psi = ((yaw - 0.5) * 80) * M_PI / 180;
-    F_des_ = ((thrust - 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;
+
     // 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);
+    // pc_->printf("%lld: thrust: %f, yaw: %f, pitch: %f, roll: %f\r\n", id, F_des_, desiredState_.psi, desiredState_.theta, desiredState_.phi);
 }