Diff: MainController.cpp

Revision:

12:7eeb29892625

Parent:

11:8ec915eb70f6

Child:

13:5ed8fd870723

--- a/MainController.cpp	Fri Jan 31 19:36:28 2014 +0000
+++ b/MainController.cpp	Sat Feb 01 00:03:40 2014 +0000
@@ -1,15 +1,19 @@
 #include "MainController.h"
 
 MainController::MainController()
-    :ch3(p16,0.054,0.092),
-     ch6(p15,0.055,0.092),
-     mcon(p22, p6, p5)
+    :ch3(p16,0.054,0.092), // frequency
+     ch4(p17,0.055,0.092), //rudder
+     ch6(p15,0.055,0.092), //volume
+     mcon(p22, p6, p5),
+     ap(p25, p26)
 {
-    wait_ms(100);
+    wait_ms(50);
     vol = 0.0;
     frq = 10.0;
+    rud = 0.5;
     frqMin = 0.5; // hardcoded
     frqMax = 3.0; //hardcoded
+    frqmxsqrt = sqrt(frqMax);
 }
 
 void MainController::control()
@@ -22,10 +26,15 @@
         timer1.reset();
         curTime = 0.0;
     }
-    amplitude = vol * frq /frqMax;
+    
+    rud = this->calculateRudder(); //not used right now
+    amplitude = vol * sqrt(frq)/frqmxsqrt;
     dutyCycle = amplitude * sin( 2.0 * MATH_PI * frq * curTime );
+    //sigm for alternative driving mode
     mcon.setpolarspeed(dutyCycle);
 }
+
+
 float MainController::calculateFrequency()
 {
     return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin);    
@@ -36,10 +45,20 @@
     return (ch6.dutycyclescaledup());
 }
 
+float MainController::calculateRudder()
+{
+    return (ch4.dutycyclescaledup());
+}
+
 void MainController::start()
 {
     timer1.start();
-    ticker1.attach(this, &MainController::control,0.0005);
+    ticker1.attach(this, &MainController::control,0.001);
+    //Autopilot guardian
+    //ap.calibrate();
+    //ap.set2D();
+    ap.setoff();
+    
 }
 
 void MainController::stop()
@@ -69,3 +88,19 @@
 {
     return vol;
 }
+
+float MainController::getRudder()
+{
+    return rud;
+}
+
+//
+//float MainController::sigm(float input)
+//{
+//    if (input>0)
+//        return 1;
+//    else if (input<0)
+//        return -1;
+//    else
+//        return 0;
+//}
\ No newline at end of file