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