File content as of revision 12:7eeb29892625:

#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
    frqmxsqrt = sqrt(frqMax);
}

void MainController::control()
{
    curTime = timer1.read();
    if(curTime > 1/frq) {
        //read new inputs
        vol = this->calculateVolume();
        frq = this->calculateFrequency();
        timer1.reset();
        curTime = 0.0;
    }
    
    rud = this->calculateRudder(); //not used right now
    amplitude = vol * sqrt(frq)/frqmxsqrt;
    dutyCycle = amplitude * sin( 2.0 * MATH_PI * frq * curTime );
    //sigm for alternative driving mode
    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::sigm(float input)
//{
//    if (input>0)
//        return 1;
//    else if (input<0)
//        return -1;
//    else
//        return 0;
//}