Diff: quadcopter.cpp

Revision:

22:92401a4fec13

Parent:

21:336faf452989

Child:

23:04338a5ef404

diff -r 336faf452989 -r 92401a4fec13 quadcopter.cpp
--- a/quadcopter.cpp	Sun Apr 10 21:47:17 2016 +0000
+++ b/quadcopter.cpp	Mon Apr 11 16:41:56 2016 +0000
@@ -9,7 +9,7 @@
 Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr)
 {   
     pc_= pcPntr;  // enable printing
-    // initSensors(accel_, mag_, gyro_, offsetAngRate_);  // IMU
+   // initSensors(accel_, mag_, gyro_, offsetAngRate_);  // IMU
 
     m_=1;
     g_=9.81;
@@ -111,25 +111,24 @@
     
     // set pwm values
     // make code faster by precomputing all the components that are used multiple times and hardcode 0.25/gamma...
-    motorPwm_.m1=0.5+ 0.25*controlInput_.f-0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
-    motorPwm_.m2=0.5 +0.25*controlInput_.f-0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz; 
-    motorPwm_.m3=0.5 + 0.25*controlInput_.f+0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
-    motorPwm_.m4=0.5 + 0.25*controlInput_.f+0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
+    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;
     
 }
 
 motors Quadcopter::getPwm()
 {
-   // motors motorPwm_
-    
+
+    return motorPwm_;
+}
 
-    
-    //old
-  //  motorPwm_.m1=0;
-   // motorPwm_.m2=0.5 + controlInput_.mx/2.5;
-    //motorPwm_.m3=0;
-    //motorPwm_.m4=0.5 - controlInput_.mx/2.5;
-    return motorPwm_;
+state Quadcopter::getState()
+{
+
+    return state_;
 }