Diff: MainController.cpp

Revision:

20:6ae16da1492a

Parent:

19:655db88b045c

Child:

22:807d5467fbf6

--- a/MainController.cpp	Wed May 28 01:48:23 2014 +0000
+++ b/MainController.cpp	Fri May 30 22:18:39 2014 +0000
@@ -1,15 +1,15 @@
 #include "MainController.h"
 
 MainController::MainController()
-    :ch1(p18,0.054,0.092), // pitch
-     ch2(p30,0.054,0.092), // roll
-     ch3(p16,0.054,0.092), // frequency
-     ch4(p17,0.055,0.092), //rudder
-     ch6(p15,0.055,0.092), //volume
+    :ch1(p18,0.056,0.090), // yaw
+     ch2(p17,0.054,0.092), // pitch
+     ch3(p15,0.054,0.092), // amplitude
+     ch4(p30,0.055,0.092), // adj
+     ch6(p16,0.053,0.092), // frequency
      mcon(p22, p6, p7), // change pin p5 to p7, because p5 is burned through
-     ap(p25, p26)//,
-     //leftservo(p21),
-     //rightservo(p22)
+     //ap(p25, p26)//,
+     leftservo(p21),
+     rightservo(p23)
      
 {
     wait_ms(50);
@@ -18,17 +18,22 @@
     frqCmd = frq;
     yaw = 0.5;
     pitch = 0.5;
-    frqMin = 0.4; // hardcoded
-    frqMax = 2.5; //hardcoded
+    adj = 0.5;
+    frqMin = 0.8; //hardcoded
+    frqMax = 2.9; //hardcoded
     //change = 0;
     //state = 0;
     fullCycle = true;
     volume = 0.0;
     volMax = 0.1;
     volChg = 0.0;
-    ampCmd = 0.0;
-    //goofftime = 0.0;
-    //switched = false;
+    raiser = 0.0;
+    pitAvg = 0.0;
+    alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
+    
+    //leftservo.calibrate(0.0008, 45); 
+    //rightservo.calibrate(-0.0001, 45); 
+
 }
 
 void MainController::control()
@@ -41,12 +46,12 @@
         // read new yaw value every half cycle
         yaw = this->calculateYaw(); // a value from -1 to 1
         
-        if(yaw < 0.075 && yaw > -0.075){
+        if(yaw < 0.1 && yaw > -0.1){
             yaw =0.0;
         }
         // calculate liquid volume change in the chambers
         volChg = volMax * yaw;
-        //volChg = 0.0;
+        volChg = 0.0;
         
         timeAdd = 0.0;
         
@@ -68,13 +73,12 @@
                     volChg = timeAdd * amp;
                 }
             }
+            ampNew = amp;
             
-//            if(yaw >0.0)
-//            {
-//                ampNew = amp + yaw*amp;
-//                ampNew = (ampNew > 1.0) ? 1.0 : ampNew;
-//                
-//            }
+            if(yaw < 0.0)
+            {
+                ampNew = (1.0+0.7*yaw)*amp;
+            }
   
             fullCycle = false;
 
@@ -94,12 +98,13 @@
                     volChg = timeAdd * amp;
                 }
             }
-//            if(yaw < 0.0)
-//            {
-//                ampNew = amp - yaw*amp;
-//                ampNew = (ampNew < -1.0) ? -1.0 : ampNew;
-//                
-//            }
+            
+            ampNew = amp;
+            
+            if(yaw > 0.0)
+            {
+                ampNew = (1.0-0.7*yaw)*amp;
+            }
             
             // use amp and frq from last cycle in order to make sure it is equalized
             fullCycle = true;
@@ -107,11 +112,13 @@
         // update the frequency that actually needs to be commanded
         frqCmd = 1.0/( 2.0*( timeAdd + 1/( 2* frq) ) );
 
+        // read new yaw value every half cycle
+        adj = this->calculateAdj(); // a value from 0 to 1
+    
         // 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
-        // ampCmd = vol * ( 2* rud + 1.0); // varied from 1 to 5      
-        ampCmd = amp * 2.0; // scale it up
+        raiser = ( 5 * adj + 1.0); // varied from 1 to 5      
         
         //reset timer
         timer1.reset();
@@ -120,33 +127,42 @@
         
       
     //Set Servo Values
-    //pitch = this->calculatePitch();
-    //leftservo = pitch;
-    //rightservo = 1.0 - pitch;
-
-    dutyCycle = saturate(ampCmd * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
+    pitch = this->calculatePitch();
+    leftservo = pitch+0.03;
+    rightservo = 1.0 - pitch;
+    if (rightservo<0.03){
+        rightservo = 0.03;
+    }
+        
+    dutyCycle = ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
             
     mcon.setpolarspeed(dutyCycle);
 }
 
 float MainController::calculateFrequency()
 {
-    return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin);    
+    return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin);    
 }
 
 float MainController::calculateAmplitude()
 {
-    return (ch6.dutycyclescaledup());
+    return (ch3.dutycyclescaledup());
 }
 
 float MainController::calculateYaw()
 {
-    return (2.0*ch4.dutycyclescaledup() - 1.0); 
+    return (2.0*ch1.dutycyclescaledup() - 1.0); 
 }
 
 float MainController::calculatePitch()
 {
-    return (ch1.dutycyclescaledup());
+    pitAvg = alPi * pitAvg+ (1.0 - alPi)*(ch2.dutycyclescaledup());
+    return pitAvg;
+}
+
+float MainController::calculateAdj()
+{
+    return (ch4.dutycyclescaledup());
 }
 
 void MainController::start()
@@ -157,7 +173,7 @@
     //Autopilot guardian
     //ap.calibrate();
     //ap.set2D();
-    ap.setoff();
+    //ap.setoff();
     
 }
 
@@ -199,6 +215,11 @@
     return pitch;
 }
 
+float MainController::getAdj()
+{
+    return adj;
+}
+
 float MainController::getTimeAdd()
 {
     return timeAdd;
@@ -224,4 +245,4 @@
         return (-1.0);
     else
         return input;
-}
\ No newline at end of file
+}