File content as of revision 21:835fd919b4bd:

#include "MainController.h"

MainController::MainController()
    :ch1(p18,0.056,0.090), // yaw
     ch2(p17,0.054,0.092), // pitch
     ch3(p15,0.054,0.092), // amplitude
     ch4(p30,0.055,0.092), // adj
     ch6(p16,0.053,0.092), // frequency
     mcon(p22, p6, p7), // change pin p5 to p7, because p5 is burned through
     //ap(p25, p26)//,
     leftservo(p21),
     rightservo(p23)
     
{
    wait_ms(50);
    amp = 0.0;
    frq = 1.0;
    frqCmd = frq;
    yaw = 0.5;
    pitch = 0.5;
    adj = 0.5;
    frqMin = 0.8; //hardcoded
    frqMax = 2.9; //hardcoded
    //change = 0;
    //state = 0;
    fullCycle = true;
    volume = 0.0;
    volMax = 0.1;
    volChg = 0.0;
    raiser = 0.0;
    pitAvg = 0.0;
    alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
    
    //leftservo.calibrate(0.0008, 45); 
    //rightservo.calibrate(-0.0001, 45); 

}

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.1 && yaw > -0.1){
            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;
                }
            }
            ampNew = amp;
            
            if(yaw < 0.0)
            {
                ampNew = (1.0+0.7*yaw)*amp;
            }
  
            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;
                }
            }
            
            ampNew = amp;
            
            if(yaw > 0.0)
            {
                ampNew = (1.0-0.7*yaw)*amp;
            }
            
            // 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) ) );

        // read new yaw value every half cycle
        adj = this->calculateAdj(); // a value from 0 to 1
    
        // 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
        raiser = ( 5 * adj + 1.0); // varied from 1 to 5      
        
        //reset timer
        timer1.reset();
        curTime = 0.0;
    }
        
      
    //Set Servo Values
    pitch = this->calculatePitch();
    leftservo = pitch+0.03;
    rightservo = 1.0 - pitch;
    if (rightservo<0.03){
        rightservo = 0.03;
    }
        
    dutyCycle = ampNew * saturate(raiser * 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) * ch6.dutycyclescaledup() + frqMin);    
}

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

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

float MainController::calculatePitch()
{
    pitAvg = alPi * pitAvg+ (1.0 - alPi)*(ch2.dutycyclescaledup());
    return pitAvg;
}

float MainController::calculateAdj()
{
    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 amp;
}


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

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

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

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

float MainController::getAdj()
{
    return adj;
}

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