polymorphic base structures for easily implementing different gaits. Triangle wave is already in there
Dependents: robotic_fish_ver_4_9_pixy
fishgait.cpp@0:aec7653a5b19, 2014-06-19 (annotated)
- Committer:
- sandwich
- Date:
- Thu Jun 19 17:53:17 2014 +0000
- Revision:
- 0:aec7653a5b19
- Child:
- 1:0aa1a6ccf5fe
works
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:aec7653a5b19 | 45 | out*=amp; |
sandwich | 0:aec7653a5b19 | 46 | if (out>1) |
sandwich | 0:aec7653a5b19 | 47 | out=1; |
sandwich | 0:aec7653a5b19 | 48 | else if (out<0) |
sandwich | 0:aec7653a5b19 | 49 | out=0; |
sandwich | 0:aec7653a5b19 | 50 | return out; |
sandwich | 0:aec7653a5b19 | 51 | } |