File content as of revision 19:655db88b045c:

#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);
    amp = 0.0;
    frq = 1.0;
    frqCmd = frq;
    yaw = 0.5;
    pitch = 0.5;
    frqMin = 0.4; // hardcoded
    frqMax = 2.5; //hardcoded
    //change = 0;
    //state = 0;
    fullCycle = true;
    volume = 0.0;
    volMax = 0.1;
    volChg = 0.0;
    ampCmd = 0.0;
    //goofftime = 0.0;
    //switched = false;
}

void MainController::control()
{
    curTime = timer1.read();
    
    // check every half cycle
    if(curTime > 1/(2*frqCmd) ) {
        
        // read new yaw value every half cycle
        yaw = this->calculateYaw(); // a value from -1 to 1
        
        if(yaw < 0.075 && yaw > -0.075){
            yaw =0.0;
        }
        // calculate liquid volume change in the chambers
        volChg = volMax * yaw;
        //volChg = 0.0;
        
        timeAdd = 0.0;
        
        // Read volume and frequency only every full cycle
        if( fullCycle ) {
            //read other new inputs
            amp = this->calculateAmplitude(); // a value from 0 to 1
            frq = this->calculateFrequency(); // a value from frqmin to frqmax

            if(volChg > 0.0) {
                // adjust frequency to add additional volume
                if( amp < 0.0001 ) {
                    amp = 0.0001;  
                } 
                timeAdd = volChg/amp;

                if( timeAdd > 0.5/frq ) {
                    timeAdd = 0.5/frq;
                    volChg = timeAdd * amp;
                }
            }
            
//            if(yaw >0.0)
//            {
//                ampNew = amp + yaw*amp;
//                ampNew = (ampNew > 1.0) ? 1.0 : ampNew;
//                
//            }
  
            fullCycle = false;

        } else {
            // reverse for the downward slope
            amp = -amp;

            if(volChg < 0.0) {
                // adjust frequency to add additional volume
                if( amp > -0.0001 ) {
                    amp = -0.0001;
                }
                timeAdd = volChg/amp;
                
                if( timeAdd > 0.5/frq ) {
                    timeAdd = 0.5/frq;
                    volChg = timeAdd * amp;
                }
            }
//            if(yaw < 0.0)
//            {
//                ampNew = amp - yaw*amp;
//                ampNew = (ampNew < -1.0) ? -1.0 : ampNew;
//                
//            }
            
            // 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 = 1.0/( 2.0*( timeAdd + 1/( 2* frq) ) );

        // 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
        // ampCmd = vol * ( 2* rud + 1.0); // varied from 1 to 5      
        ampCmd = amp * 2.0; // scale it up
        
        //reset timer
        timer1.reset();
        curTime = 0.0;
    }
        
      
    //Set Servo Values
    //pitch = this->calculatePitch();
    //leftservo = pitch;
    //rightservo = 1.0 - pitch;

    dutyCycle = saturate(ampCmd * sin( 2.0 * MATH_PI * frqCmd * curTime )); // add factor 4.0 to get a cut off sinus
            
    mcon.setpolarspeed(dutyCycle);
}

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

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

float MainController::calculateYaw()
{
    return (2.0*ch4.dutycyclescaledup() - 1.0); 
}

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


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

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

float MainController::getYaw()
{
    return yaw;
}

float MainController::getPitch()
{
    return pitch;
}

float MainController::getTimeAdd()
{
    return timeAdd;
}

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