File content as of revision 18:9ba4566f2361:

#include "MainController.h"

MainController::MainController()
    :ch1(p18,0.054,0.092), // pitch
     ch2(p30,0.054,0.092), // roll
     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 pin p5 to p7, because p5 is burned through
     ap(p25, p26)//,
     //leftservo(p21),
     //rightservo(p22)
     
{
    wait_ms(50);
    vol = 0.0;
    frq = 1.0;
    rud = 0.5;
    pitch = 0.5;
    frqMin = 0.4; // hardcoded
    frqMax = 2.5; //hardcoded
    change = 0;
    state = 0;
    //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();
        timer1.reset();
        curTime = 0.0;

        // rudder value used to define the trapezoid shape
        // amplitude = vol * ( 2* rud + 1.0); // varied from 1 to 5
        amplitude = vol * 4.0;
        
        //reset change to zero
        change = 0;
        
        if(state == 1) {
            if(rud < 0.75) {
                change = -1;
                state = 0;
            }
        } 
        else if (state == -1) {
            if(rud> 0.25) {
                change = 1;
                state = 0;
            }
        } 
        else {
            if(rud < 0.25) {
                change = -1;
                state = -1;
            } else if(rud > 0.75) {
                change = 1;
                state = 1;
            }
        }
        //switched = false;
        //goofftime = (vol/(2*frq));    
    }
    
    //Set Servo Values
    //pitch = this->calculatePitch();
    //leftservo = pitch;
    //rightservo = 1.0 - pitch;
       
//    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;
//    }

    // saturate ensures the duty cycle does not exceed 1, 
    
    if((change == 1) && (curTime > 1/(2*frq))) {
        dutyCycle = 0.0;
    } else if((change == -1) && (curTime < 1/(2*frq))) {
        dutyCycle = 0.0;
    } else {
        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());
}

float MainController::calculatePitch()
{
    return (ch1.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::getPitch()
{
    return pitch;
}

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