File content as of revision 16:79cfe6201318:

#include "MainController.h"

MainController::MainController()
    :ch3(p16,0.054,0.092), // frequency
     ch4(p17,0.055,0.092), //rudder
     ch6(p15,0.055,0.092), //volume
     mcon(p22, p6, p7), // change p5 to p7, because p5 is burned through
     ap(p25, p26)
{
    wait_ms(50);
    vol = 0.0;
    frq = 10.0;
    rud = 0.5;
    frqMin = 0.4; // hardcoded
    frqMax = 2.5; //hardcoded
    goofftime = 0.0;
    switched = false;
}

void MainController::control()
{
    curTime = timer1.read();
    
    if(curTime > 1/frq) {
        //read new inputs
        vol = this->calculateVolume();
        frq = this->calculateFrequency();
        rud = this->calculateRudder(); //not used right now
        timer1.reset();
        curTime = 0.0;
        
        // rudder value used to define the trapezoid shape
        amplitude = vol * ( 2* rud + 1.0);
        //amplitude = vol * 3.0;
        
        switched = false;
        goofftime = (vol/(2*frq));
            
    }
       
//    if (curTime > 1/(2*frq) && (switched == false))
//    {
//        switched = true;
//        amplitude = -1.0;
//     }
//        
//    if(!switched)
//    {
//        if(curTime > goofftime )
//        amplitude = 0.0;
//    }
//    else
//    {
//        if(curTime > (1/(2*frq)+goofftime))
//        amplitude = 0.0;
//    }

    dutyCycle = saturate(amplitude * sin( 2.0 * MATH_PI * frq * curTime ));

    mcon.setpolarspeed(dutyCycle);
}


float MainController::calculateFrequency()
{
    return ((frqMax - frqMin) * ch3.dutycyclescaledup() + frqMin);    
}

float MainController::calculateVolume()
{
    return (ch6.dutycyclescaledup());
}

float MainController::calculateRudder()
{
    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();
    mcon.setpolarspeed(0.0);
}

float MainController::getDutyCycle()
{
    return dutyCycle;
}

float MainController::getAmplitude()
{
    return amplitude;
}


float MainController::getFrequency()
{
    return frq;
}

float MainController::getVolume()
{
    return vol;
}

float MainController::getRudder()
{
    return rud;
}

//signum function
float MainController::signum(float input)
{
    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)
{
    if (input > 1.0)
        return (1.0);
    else if (input < -1.0)
        return (-1.0);
    else
        return input;
}