jetfishteam / fishgait

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

Dependents:   robotic_fish_ver_4_9_pixy

fishgait.cpp@1:0aa1a6ccf5fe, 2014-06-19 (annotated)

Committer:
sandwich
Date:
Thu Jun 19 19:51:59 2014 +0000
Revision:
1:0aa1a6ccf5fe
Parent:
0:aec7653a5b19
Child:
2:d95c40ac8a3e
fixed a bug where amplitude would be incorrect because wave isn't centered on 0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sandwich 0:aec7653a5b19 1 #include "fishgait.h"
sandwich 0:aec7653a5b19 2
sandwich 0:aec7653a5b19 3 fishgait::fishgait()
sandwich 0:aec7653a5b19 4 {
sandwich 0:aec7653a5b19 5 t=new Timer();
sandwich 0:aec7653a5b19 6 t->start();
sandwich 0:aec7653a5b19 7 }
sandwich 0:aec7653a5b19 8
sandwich 0:aec7653a5b19 9 fishgait::~fishgait()
sandwich 0:aec7653a5b19 10 {
sandwich 0:aec7653a5b19 11 t->stop();
sandwich 0:aec7653a5b19 12 delete t;
sandwich 0:aec7653a5b19 13 }
sandwich 0:aec7653a5b19 14
sandwich 0:aec7653a5b19 15 void fishgait::setTimer(Timer* tObject)
sandwich 0:aec7653a5b19 16 {
sandwich 0:aec7653a5b19 17 delete t; //not sure if this is safe
sandwich 0:aec7653a5b19 18 t=NULL;
sandwich 0:aec7653a5b19 19 t=tObject;
sandwich 0:aec7653a5b19 20 }
sandwich 0:aec7653a5b19 21
sandwich 0:aec7653a5b19 22 triangleGait::triangleGait(float freq, float amplitude) : fishgait(), frq(freq), amp(amplitude)
sandwich 0:aec7653a5b19 23 {
sandwich 0:aec7653a5b19 24 }
sandwich 0:aec7653a5b19 25 /*
sandwich 0:aec7653a5b19 26 triangleGait::~triangleGait()
sandwich 0:aec7653a5b19 27 {
sandwich 0:aec7653a5b19 28 }
sandwich 0:aec7653a5b19 29 */
sandwich 0:aec7653a5b19 30
sandwich 0:aec7653a5b19 31 float triangleGait::compute()
sandwich 0:aec7653a5b19 32 {
sandwich 0:aec7653a5b19 33 float halfperiod=(1.0f/(frq/2.0f))*1000; //the half period in ms
sandwich 0:aec7653a5b19 34 int curtime=t->read_ms(); //read time
sandwich 0:aec7653a5b19 35 static bool direction=true; //true-> up, false->down
sandwich 0:aec7653a5b19 36 float out=0;
sandwich 0:aec7653a5b19 37 if (curtime>halfperiod) {
sandwich 0:aec7653a5b19 38 direction=!direction;
sandwich 0:aec7653a5b19 39 t->reset();
sandwich 0:aec7653a5b19 40 }
sandwich 0:aec7653a5b19 41 if (direction==true)
sandwich 0:aec7653a5b19 42 out=float(curtime)/halfperiod;
sandwich 0:aec7653a5b19 43 else if (direction==false)
sandwich 0:aec7653a5b19 44 out=1.0f-(float(curtime)/halfperiod);
sandwich 1:0aa1a6ccf5fe 45 out-=0.5; //recenter the wave around 0 instead of 0.5
sandwich 1:0aa1a6ccf5fe 46 out*=amp; //factor in amplitude
sandwich 1:0aa1a6ccf5fe 47 out+=0.5; //recenter around 0.5
sandwich 0:aec7653a5b19 48 if (out>1)
sandwich 0:aec7653a5b19 49 out=1;
sandwich 0:aec7653a5b19 50 else if (out<0)
sandwich 0:aec7653a5b19 51 out=0;
sandwich 0:aec7653a5b19 52 return out;
sandwich 1:0aa1a6ccf5fe 53 }
sandwich 1:0aa1a6ccf5fe 54
sandwich 1:0aa1a6ccf5fe 55 squareGait::squareGait(float freq, float amplitude) : fishgait(), frq(freq), amp(amplitude)
sandwich 1:0aa1a6ccf5fe 56 {
sandwich 1:0aa1a6ccf5fe 57 }
sandwich 1:0aa1a6ccf5fe 58 /*
sandwich 1:0aa1a6ccf5fe 59 triangleGait::~triangleGait()
sandwich 1:0aa1a6ccf5fe 60 {
sandwich 1:0aa1a6ccf5fe 61 }
sandwich 1:0aa1a6ccf5fe 62 */
sandwich 1:0aa1a6ccf5fe 63
sandwich 1:0aa1a6ccf5fe 64 float squareGait::compute()
sandwich 1:0aa1a6ccf5fe 65 {
sandwich 1:0aa1a6ccf5fe 66 float halfperiod=(1.0f/(frq/2.0f))*1000; //the half period in ms
sandwich 1:0aa1a6ccf5fe 67 int curtime=t->read_ms(); //read time
sandwich 1:0aa1a6ccf5fe 68 static bool direction=true; //true-> up, false->down
sandwich 1:0aa1a6ccf5fe 69 float out=0;
sandwich 1:0aa1a6ccf5fe 70 if (curtime>halfperiod) {
sandwich 1:0aa1a6ccf5fe 71 direction=!direction;
sandwich 1:0aa1a6ccf5fe 72 t->reset();
sandwich 1:0aa1a6ccf5fe 73 }
sandwich 1:0aa1a6ccf5fe 74 if (direction==true)
sandwich 1:0aa1a6ccf5fe 75 out=1.0f;
sandwich 1:0aa1a6ccf5fe 76 else if (direction==false)
sandwich 1:0aa1a6ccf5fe 77 out=0.0f;
sandwich 1:0aa1a6ccf5fe 78 out*=amp;
sandwich 1:0aa1a6ccf5fe 79 return out;
sandwich 0:aec7653a5b19 80 }