File content as of revision 2:040b8c8f4f92:

#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)

{
    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;
    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)-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;
    
}

//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;
}