File content as of revision 13:5ed8fd870723:

#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, p5),
     ap(p25, p26)
{
    wait_ms(50);
    vol = 0.0;
    frq = 10.0;
    rud = 0.5;
    frqMin = 0.5; // hardcoded
    frqMax = 3.0; //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;
        switched = false;
        goofftime = (1/(2*frq)*vol);
        amplitude = 1.0;
        
    }
    
    //vol * sqrt(frq)/frqmxsqrt;
    
    if (curTime > 1/(2*frq))
    {
        switched = true;
        amplitude = -1.0;
     }
        
    if(!switched)
    {
        if(curTime > goofftime )
        amplitude = 0.0;
    }
    else
    {
        if(curTime > (1/(2*frq)+goofftime))
        amplitude = 0.0;
    }

    dutyCycle = amplitude * signum(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;
}

//
float MainController::signum(float input)
{
    if (input>0.0)
        return 1;
    else if (input<0.0)
        return -1;
    else
        return 0;
}