Diff: MainController.cpp

Revision:

3:605f216167f6

Parent:

2:040b8c8f4f92

--- a/MainController.cpp	Thu Jul 30 20:36:00 2015 +0000
+++ b/MainController.cpp	Fri Feb 26 14:35:02 2016 +0000
@@ -1,137 +1,32 @@
 #include "MainController.h"
 
 MainController::MainController()
-    :ch1(p17,0.0636,0.1075), // yaw
-     ch2(p15,0.0623,0.1075), // pitch
-     ch3(p18,0.0614,0.1071), // amplitude
-     //ch4(p30,0.055,0.092), // adj
-     ch6(p16,0.0625,0.1055), // frequency
-     esc1(p25)
-     //ap(p25, p26)//,
-     //leftservo(p21),
-     //rightservo(p23)
-
+    :esc1(p25),
+    myled(LED1),
+    mypotentiometer(p20)
 {
     wait_ms(50);
     amp = 0.0;
-    frq = 1.0;
-    frqCmd = frq;
-    yaw = 0.5;
     frqMin = 0.8; //hardcoded
     frqMax = 1.8; //hardcoded
-    fullCycle = true;
-    raiser = 1.0;
-    alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
-
-    throttle_var = 0.5; //neutral position
-    esc1 = throttle_var;
+    dutyCycle = 0.5;
+    esc1 = dutyCycle;
     esc1();
     wait_ms(3000); //to arm brushless motor
-    //leftservo.calibrate(0.0008, 45);
-    //rightservo.calibrate(-0.0001, 45);
 
 }
 
 void MainController::control()
 {
+    amp = mypotentiometer.read();
+    myled = amp;
+        
     curTime = timer1.read();
-
-    // check every half cycle
-    if(curTime > (1/(2*frqCmd)-TOFF) ) {
-
-        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
-
-            if(yaw < 0.1 && yaw > -0.1) { // eliminating noise around 0.0 +-0.1
-                yaw =0.0;
-            }
-
-            // 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
-
-                ampNew = amp;
-
-                if(yaw < 0.0) {
-                    ampNew = (1.0+0.7*yaw)*amp;
-                }
-
-                fullCycle = false;
-
-            } else {
-                // reverse for the downward slope
-                amp = -amp;
-
-                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;
-            }
-            // update the frequency that actually needs to be commanded
-            frqCmd = 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
-            raiser = 1.5; // varied from 1 to 5
-
-            //reset timer
-            timer1.reset();
-            curTime = 0.0;
-        }
-    }
-
-
-//    //Set Servo Values
-    pitch = this->calculatePitch();
-//    leftservo = pitch+0.03;
-//    rightservo = 1.0 - pitch;
-//    if (rightservo<0.03){
-//        rightservo = 0.03;
-//    }
-
-    dutyCycle = 0.5 + ampNew * saturate(raiser * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
+    
     //set brushless motor speed
-    throttle_var = dutyCycle;
-    esc1 = throttle_var;
-    esc1();
-}
-
-float MainController::calculateFrequency()
-{
-    return ((frqMax - frqMin) * ch6.dutycyclescaledup() + frqMin);
-    //return (ch6.dutycyclescaledup());
-}
-
-float MainController::calculateAmplitude()
-{
-    return (ch3.dutycyclescaledup() / 2.0);
-}
-
-float MainController::calculateYaw()
-{
-    return (2.0*ch1.dutycyclescaledup() - 1.0); //maps from 0.0-1.0 to -1.0 - 1.0
-    //return (ch1.dutycyclescaledup());
-}
-
-float MainController::calculatePitch()
-{
-    //float pitAvg = alPi * pitAvg+ (1.0 - alPi)*(ch2.dutycyclescaledup());
-    float pitAvg = (ch2.dutycyclescaledup());
-    return pitAvg;
-    
+    dutyCycle = 0.5 + (amp/2); 
+    esc1.setThrottle(dutyCycle);
+    esc1.pulse();
 }
 
 //float MainController::calculateAdj()
@@ -144,19 +39,14 @@
     timer1.start();
 
     ticker1.attach(this, &MainController::control,0.001);
-    //Autopilot guardian
-    //ap.calibrate();
-    //ap.set2D();
-    //ap.setoff();
-
 }
 
 void MainController::stop()
 {
     timer1.stop();
     ticker1.detach();
-    throttle_var = 0.5;
-    esc1 = throttle_var;
+    dutyCycle = 0.5;
+    esc1 = dutyCycle;
     esc1();
 }
 
@@ -170,22 +60,6 @@
     return amp;
 }
 
-
-float MainController::getFrequency()
-{
-    return frq;
-}
-
-float MainController::getYaw()
-{
-    return yaw;
-}
-
-float MainController::getPitch()
-{
-    return pitch;
-}
-
 //signum function
 float MainController::signum(float input) //gives back the sign
 {