polymorphic base structures for easily implementing different gaits. Triangle wave is already in there

Dependents:   robotic_fish_ver_4_9_pixy

fishgait.cpp

Committer:
sandwich
Date:
2014-06-19
Revision:
2:d95c40ac8a3e
Parent:
1:0aa1a6ccf5fe

File content as of revision 2:d95c40ac8a3e:

#include "fishgait.h"

fishgait::fishgait()
{
    t=new Timer();
    t->start();
}

fishgait::~fishgait()
{
    t->stop();
    delete t;
}

void fishgait::setTimer(Timer* tObject)
{
    delete t; //not sure if this is safe
    t=NULL;
    t=tObject;
}
/*
mixedGait::mixedGait() : count(0)
{
}

void mixedGait::attach(fishgait gait, float weight)
{
    gaits[count]=gait;
    weights[count]=weight;
    ++count;
    return;
}

void mixedGait::compute()
{
    float out=0;
    if (count==0) //derp
        return 0.5;
    //do the mixing
    for (int i=0; i<count; ++i) {
        out+=gaits[count].compute()*weights[count];
    }
    return out;
}
*/
triangleGait::triangleGait(float freq, float amplitude) : fishgait(), frq(freq), amp(amplitude)
{
}
/*
triangleGait::~triangleGait()
{
}
*/

float triangleGait::compute()
{
    float halfperiod=(1.0f/(frq/2.0f))*1000; //the half period in ms
    int curtime=t->read_ms(); //read time
    static bool direction=true; //true-> up, false->down
    float out=0;
    if (curtime>halfperiod) {
        direction=!direction;
        t->reset();
    }
    if (direction==true)
        out=float(curtime)/halfperiod;
    else if (direction==false)
        out=1.0f-(float(curtime)/halfperiod);
    out-=0.5; //recenter the wave around 0 instead of 0.5
    out*=amp; //factor in amplitude
    out+=0.5; //recenter around 0.5
    if (out>1)
        out=1;
    else if (out<0)
        out=0;
    return out;
}

squareGait::squareGait(float freq, float amplitude) : fishgait(), frq(freq), amp(amplitude)
{
}
/*
triangleGait::~triangleGait()
{
}
*/

float squareGait::compute()
{
    float halfperiod=(1.0f/(frq/2.0f))*1000; //the half period in ms
    int curtime=t->read_ms(); //read time
    static bool direction=true; //true-> up, false->down
    float out=0;
    if (curtime>halfperiod) {
        direction=!direction;
        t->reset();
    }
    if (direction==true)
        out=0.5f+0.5*amp;
    if (out>1)
        out=1;
    else if (direction==false)
        out=0.5f-0.5*amp;
    if (out<0)
        out=0;
    return out;
}

sawGait::sawGait(float freq, float amplitude) : fishgait(), frq(freq), amp(amplitude)
{
}

float sawGait::compute()
{
    float period=(1.0f/frq)*1000; //the period in ms
    int curtime=t->read_ms(); //read time
    float out=0;
    if (curtime>period) {
        t->reset();
    }
    out=float(curtime)/period;
    out-=0.5; //recenter the wave around 0 instead of 0.5
    out*=amp; //factor in amplitude
    out+=0.5; //recenter around 0.5
    if (out>1)
        out=1;
    else if (out<0)
        out=0;
    return out;
}