Diff: MainController.cpp

Revision:

2:040b8c8f4f92

Parent:

0:187bb46ed128

Child:

3:605f216167f6

--- a/MainController.cpp	Wed Jul 29 20:03:01 2015 +0000
+++ b/MainController.cpp	Thu Jul 30 20:36:00 2015 +0000
@@ -6,7 +6,7 @@
      ch3(p18,0.0614,0.1071), // amplitude
      //ch4(p30,0.055,0.092), // adj
      ch6(p16,0.0625,0.1055), // frequency
-     esc1(p21)
+     esc1(p25)
      //ap(p25, p26)//,
      //leftservo(p21),
      //rightservo(p23)
@@ -17,10 +17,10 @@
     frq = 1.0;
     frqCmd = frq;
     yaw = 0.5;
-    frqMin = 0.7; //hardcoded
+    frqMin = 0.8; //hardcoded
     frqMax = 1.8; //hardcoded
     fullCycle = true;
-    raiser = 0.0;
+    raiser = 1.0;
     alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
 
     throttle_var = 0.5; //neutral position
@@ -37,58 +37,60 @@
     curTime = timer1.read();
 
     // check every half cycle
-    if(curTime > 1/(2*frqCmd) ) {
-
-        // read new yaw value every half cycle
-        yaw = this->calculateYaw(); // a value from -1 to 1
-
-        if(yaw < 0.1 && yaw > -0.1){ // eliminating noise around 0.0 +-0.1
-            yaw =0.0;
-        }
+    if(curTime > (1/(2*frqCmd)-TOFF) ) {
 
-        // Read volume and frequency only every full cycle
-        if( fullCycle ) {
-            //read other new inputs only at upward portion of full cycle
-            amp = this->calculateAmplitude(); // a value from 0 to 1
-            frq = this->calculateFrequency(); // a value from frqmin to frqmax
+        if(curTime < 1/(2*frqCmd) ) {
+            ampNew = 0.0;
+        } else {
+            // read new yaw value every half cycle
+            yaw = this->calculateYaw(); // a value from -1 to 1
 
-            ampNew = amp;
-
-            if(yaw < 0.0)
-            {
-                ampNew = (1.0+0.7*yaw)*amp;
+            if(yaw < 0.1 && yaw > -0.1) { // eliminating noise around 0.0 +-0.1
+                yaw =0.0;
             }
 
-            fullCycle = false;
+            // Read volume and frequency only every full cycle
+            if( fullCycle ) {
+                //read other new inputs only at upward portion of full cycle
+                amp = this->calculateAmplitude(); // a value from 0 to 1
+                frq = this->calculateFrequency(); // a value from frqmin to frqmax
 
-        } else {
-            // reverse for the downward slope
-            amp = -amp;
+                ampNew = amp;
 
-            ampNew = amp;
+                if(yaw < 0.0) {
+                    ampNew = (1.0+0.7*yaw)*amp;
+                }
+
+                fullCycle = false;
 
-            if(yaw > 0.0)
-            {
-                ampNew = (1.0-0.7*yaw)*amp;
-            }
+            } else {
+                // reverse for the downward slope
+                amp = -amp;
+
+                ampNew = amp;
 
-            // use amp and frq from last cycle in order to make sure it is equalized
-            fullCycle = true;
-        }
-        // update the frequency that actually needs to be commanded
-        frqCmd = frq;
+                if(yaw > 0.0) {
+                    ampNew = (1.0-0.7*yaw)*amp;
+                }
 
-        // read new yaw value every half cycle
-        //adj = this->calculateAdj(); // a value from 0 to 1
+                // use amp and frq from last cycle in order to make sure it is equalized
+                fullCycle = true;
+            }
+            // update the frequency that actually needs to be commanded
+            frqCmd = frq;
 
-        // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller
-        //volume = volChg + volume;
-        // rudder value used to define the trapezoid shape
-        raiser = 5; // varied from 1 to 5
+            // read new yaw value every half cycle
+            //adj = this->calculateAdj(); // a value from 0 to 1
 
-        //reset timer
-        timer1.reset();
-        curTime = 0.0;
+            // for keeping track, calculate current volume storage, positive means on side is fuller, negative means other side is fuller
+            //volume = volChg + volume;
+            // rudder value used to define the trapezoid shape
+            raiser = 1.5; // varied from 1 to 5
+
+            //reset timer
+            timer1.reset();
+            curTime = 0.0;
+        }
     }