Diff: MainController.cpp

Revision:

0:187bb46ed128

Child:

2:040b8c8f4f92

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MainController.cpp	Wed Jul 29 20:00:59 2015 +0000
@@ -0,0 +1,207 @@
+#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(p21)
+     //ap(p25, p26)//,
+     //leftservo(p21),
+     //rightservo(p23)
+
+{
+    wait_ms(50);
+    amp = 0.0;
+    frq = 1.0;
+    frqCmd = frq;
+    yaw = 0.5;
+    frqMin = 0.7; //hardcoded
+    frqMax = 1.8; //hardcoded
+    fullCycle = true;
+    raiser = 0.0;
+    alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
+
+    throttle_var = 0.5; //neutral position
+    esc1 = throttle_var;
+    esc1();
+    wait_ms(3000); //to arm brushless motor
+    //leftservo.calibrate(0.0008, 45);
+    //rightservo.calibrate(-0.0001, 45);
+
+}
+
+void MainController::control()
+{
+    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;
+        }
+
+        // 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 = 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;
+    
+}
+
+//float MainController::calculateAdj()
+//{
+//    return (ch4.dutycyclescaledup());
+//}
+
+void MainController::start()
+{
+    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;
+    esc1();
+}
+
+float MainController::getDutyCycle()
+{
+    return dutyCycle;
+}
+
+float MainController::getAmplitude()
+{
+    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
+{
+    if (input>0.0)
+        return 1.0;
+    else if (input<0.0)
+        return (-1.0);
+    else
+        return 0.0;
+}
+
+//saturate function
+float MainController::saturate(float input) //saturates a value at 1.0 or -1.0
+{
+    if (input > 1.0)
+        return (1.0);
+    else if (input < -1.0)
+        return (-1.0);
+    else
+        return input;
+}
\ No newline at end of file