File content as of revision 25:4f2f441eceec:

#include "MainController.h"

MainController::MainController()
    :ch1(p18,0.047,0.081), // yaw
     ch2(p17,0.047,0.09), // pitch
     ch3(p15,0.047,0.081), // amplitude
     ch4(p30,0.047,0.081), // adj
     ch6(p16,0.047,0.081), // frequency
     ch5(p29,0.047,0.081), // manual vs auto control
     syren(p9,p10), //syren
     //ap(p25, p26)//,
     leftservo(p21),
     rightservo(p24),
     pcam(p11,p12,p13,10),
     controlThread(&MainController::controlThreadStarter, this, osPriorityNormal, 1024),
     trackerThread(&MainController::trackerThreadStarter, this, osPriorityNormal, 1024),
     panLoop(300,100), //pgain, dgain
     tiltLoop(500,900) //pgain, dgain
     //controlTimer(&MainController::control, osTimerPeriodic, (void*) 0)
{
    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;

    yawAdjVal = 0.7;

    raiser = 0.0;
    pitAvg = 0.0;
    alPi = 0.2/(0.2+0.001);//tf/(tf+Ts);
    syren.baud(9600);
    dutyCycle=127;
    //pcam=new pixySPI(p11,p12,p13,10);

    //leftservo.calibrate(0.0008, 45);
    //rightservo.calibrate(-0.0001, 45);

}

MainController::~MainController()
{
}

/*
void MainController::dutyThreadStarter(void const* p)
{
    MainController *instance = (MainController*)p;
    instance->updateDutyCycle();
}
*/

void MainController::controlThreadStarter(void const* p)
{
    MainController *instance = (MainController*)p;
    instance->control();
}


void MainController::trackerThreadStarter(void const *p)
{
    MainController *instance = (MainController*)p;
    instance->trackTarget();
}

/*
void MainController::updateDutyCycle()
{
    dutyThread.signal_wait(START_THREAD);
    while (1) {
        dutyMutex.lock();
        syren.putc(int(dutyCycle));
        dutyMutex.unlock();
    }
}
*/
void MainController::control()
{
    controlThread.signal_wait(START_THREAD);
    while (1) {
        //
        //while (1) {
        //trackMutex.lock();
        curTime = timer1.read();
        //Block bestObject=pcam.getBlocks()[0]; //our best bet is the first block because it has the biggest area
        bool autonomous=getOpMode();

        // check every half cycle
        if(curTime > 1/(2*frqCmd) ) {
            // read new yaw value every half cycle
            if (autonomous==false) {
                yaw = this->calculateYaw(); // a value from -1 to 1
                if(yaw < 0.1 && yaw > -0.1)
                    yaw =0.0;
            } else {
                trackMutex.lock();
                int error=CENTER_X-pcam.getBestX(); //calculate yaw difference
                trackMutex.unlock();
                panLoop.update(error);
                yaw=(float(panLoop.m_pos)/1000.0f)*2-1.00f; //scale from (0,1000) -> (-1,1)
            }


            // 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(yaw < 0.0) {
                    ampCmd = (1.0+ yawAdjVal * yaw)*amp;
                } else {
                    ampCmd = amp;
                }

                fullCycle = false;

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



                if(yaw > 0.0) {
                    ampCmd = (1.0- yawAdjVal *yaw)*amp;
                } else {
                    ampCmd = 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 = frq;

            // read new yaw value every half cycle
            adj = this->calculateAdj(); // a value from 0 to 1


            // adj value used to define the trapezoid shape
            raiser = ( 5 * 0.7 + 1.0); // varied from 1 to 5 for now hard coded

            //reset timer
            timer1.reset();
            curTime = 0.0;
        }


        //Set Servo Values
        if (autonomous==false)
            pitch = this->calculatePitch();
        else {
            trackMutex.lock();
            int error=pcam.getBestY()-CENTER_Y;
            trackMutex.unlock();
            tiltLoop.update(error);
            pitch=float(tiltLoop.m_pos)/1000.0f;
            //printf("Y error: %d, pitch: %f\n", error, pitch);
        }

        //Adjusting the trim of the pitch angles
        leftservo = pitch+0.03;
        if (leftservo > 1.0) {
            leftservo = 1.0;
        }
        rightservo = 1.0 - pitch;
        if (rightservo < 0.03) {
            rightservo = 0.03;
        }


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

        //now scale duty cycle from -1 to 1 -> 0 to 255


        dutyCycle=(255/2)*dutyCycle+127;
        if (dutyCycle<0)
            dutyCycle=0;
        else if (dutyCycle>255)
            dutyCycle=255;
        syren.putc(uint16_t(dutyCycle));
        //Thread::wait(1);
        //dutyMutex.unlock();
    }
}

void MainController::trackTarget()
{
    trackerThread.signal_wait(START_THREAD);
    while (1) {
        if (getOpMode()==true) {
            trackMutex.lock();
            pcam.capture();
            trackMutex.unlock();
        }
        Thread::wait(100);
    }
}

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();

    printf("start ticker\n");
    //ticker1.attach(this, &MainController::control,0.001);
    /*
    while (1)
    {
        syren.putc(int(dutyCycle));
    }
    */
    controlThread.signal_set(START_THREAD);
    //dutyThread.signal_set(START_THREAD);
    trackerThread.signal_set(START_THREAD);
    //controlTimer.start(2);
    //control=Thread(&MainController::control);
    //tracker.attach(this, &MainController::trackTarget, 0.1); //update target position
    //Autopilot guardian
    //ap.calibrate();
    //ap.set2D();
    //ap.setoff();
    //RtosTimer controlTimer((void*)(&control), osTimerPeriodic, (void *)0);
    //controlTimer.start(5);
    //while(true) {
    //printf("control: %d, frq: %.3f, vol: %.2f, amp: %.3f, yaw: %.2f, dut: %.2f, pit: %.2f, adj: %.2f, t: %.2f\n",
    //          mainCtrl.getOpMode(), mainCtrl.getFrequency(), mainCtrl.getVolume(), mainCtrl.getAmplitude(), mainCtrl.getYaw(), mainCtrl.getDutyCycle(), mainCtrl.getPitch(), mainCtrl.getAdj(), t.read());

    //Thread::wait(100);
    //wait_ms(100);


}

void MainController::stop()
{
    timer1.stop();
    //ticker1.detach();
    //tracker.detach();
    //mcon.setpolarspeed(0.0);
    syren.putc(int(127));
}

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

bool MainController::getOpMode()
{
    return ch5.dutycyclescaledup()>0.5;
}

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