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@2:d95c40ac8a3e, 2014-06-19 (annotated)

Committer:
sandwich
Date:
Thu Jun 19 21:04:19 2014 +0000
Revision:
2:d95c40ac8a3e
Parent:
1:0aa1a6ccf5fe 
sawtooth gait works. working on mixed signals gait;

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 2:d95c40ac8a3e 21 /*
sandwich 2:d95c40ac8a3e 22 mixedGait::mixedGait() : count(0)
sandwich 2:d95c40ac8a3e 23 {
sandwich 2:d95c40ac8a3e 24 }
sandwich 0:aec7653a5b19 25
sandwich 2:d95c40ac8a3e 26 void mixedGait::attach(fishgait gait, float weight)
sandwich 2:d95c40ac8a3e 27 {
sandwich 2:d95c40ac8a3e 28 gaits[count]=gait;
sandwich 2:d95c40ac8a3e 29 weights[count]=weight;
sandwich 2:d95c40ac8a3e 30 ++count;
sandwich 2:d95c40ac8a3e 31 return;
sandwich 2:d95c40ac8a3e 32 }
sandwich 2:d95c40ac8a3e 33
sandwich 2:d95c40ac8a3e 34 void mixedGait::compute()
sandwich 2:d95c40ac8a3e 35 {
sandwich 2:d95c40ac8a3e 36 float out=0;
sandwich 2:d95c40ac8a3e 37 if (count==0) //derp
sandwich 2:d95c40ac8a3e 38 return 0.5;
sandwich 2:d95c40ac8a3e 39 //do the mixing
sandwich 2:d95c40ac8a3e 40 for (int i=0; i<count; ++i) {
sandwich 2:d95c40ac8a3e 41 out+=gaits[count].compute()*weights[count];
sandwich 2:d95c40ac8a3e 42 }
sandwich 2:d95c40ac8a3e 43 return out;
sandwich 2:d95c40ac8a3e 44 }
sandwich 2:d95c40ac8a3e 45 */
sandwich 0:aec7653a5b19 46 triangleGait::triangleGait(float freq, float amplitude) : fishgait(), frq(freq), amp(amplitude)
sandwich 0:aec7653a5b19 47 {
sandwich 0:aec7653a5b19 48 }
sandwich 0:aec7653a5b19 49 /*
sandwich 0:aec7653a5b19 50 triangleGait::~triangleGait()
sandwich 0:aec7653a5b19 51 {
sandwich 0:aec7653a5b19 52 }
sandwich 0:aec7653a5b19 53 */
sandwich 0:aec7653a5b19 54
sandwich 0:aec7653a5b19 55 float triangleGait::compute()
sandwich 0:aec7653a5b19 56 {
sandwich 0:aec7653a5b19 57 float halfperiod=(1.0f/(frq/2.0f))*1000; //the half period in ms
sandwich 0:aec7653a5b19 58 int curtime=t->read_ms(); //read time
sandwich 0:aec7653a5b19 59 static bool direction=true; //true-> up, false->down
sandwich 0:aec7653a5b19 60 float out=0;
sandwich 0:aec7653a5b19 61 if (curtime>halfperiod) {
sandwich 0:aec7653a5b19 62 direction=!direction;
sandwich 0:aec7653a5b19 63 t->reset();
sandwich 0:aec7653a5b19 64 }
sandwich 0:aec7653a5b19 65 if (direction==true)
sandwich 0:aec7653a5b19 66 out=float(curtime)/halfperiod;
sandwich 0:aec7653a5b19 67 else if (direction==false)
sandwich 0:aec7653a5b19 68 out=1.0f-(float(curtime)/halfperiod);
sandwich 1:0aa1a6ccf5fe 69 out-=0.5; //recenter the wave around 0 instead of 0.5
sandwich 1:0aa1a6ccf5fe 70 out*=amp; //factor in amplitude
sandwich 1:0aa1a6ccf5fe 71 out+=0.5; //recenter around 0.5
sandwich 0:aec7653a5b19 72 if (out>1)
sandwich 0:aec7653a5b19 73 out=1;
sandwich 0:aec7653a5b19 74 else if (out<0)
sandwich 0:aec7653a5b19 75 out=0;
sandwich 0:aec7653a5b19 76 return out;
sandwich 1:0aa1a6ccf5fe 77 }
sandwich 1:0aa1a6ccf5fe 78
sandwich 1:0aa1a6ccf5fe 79 squareGait::squareGait(float freq, float amplitude) : fishgait(), frq(freq), amp(amplitude)
sandwich 1:0aa1a6ccf5fe 80 {
sandwich 1:0aa1a6ccf5fe 81 }
sandwich 1:0aa1a6ccf5fe 82 /*
sandwich 1:0aa1a6ccf5fe 83 triangleGait::~triangleGait()
sandwich 1:0aa1a6ccf5fe 84 {
sandwich 1:0aa1a6ccf5fe 85 }
sandwich 1:0aa1a6ccf5fe 86 */
sandwich 1:0aa1a6ccf5fe 87
sandwich 1:0aa1a6ccf5fe 88 float squareGait::compute()
sandwich 1:0aa1a6ccf5fe 89 {
sandwich 1:0aa1a6ccf5fe 90 float halfperiod=(1.0f/(frq/2.0f))*1000; //the half period in ms
sandwich 1:0aa1a6ccf5fe 91 int curtime=t->read_ms(); //read time
sandwich 1:0aa1a6ccf5fe 92 static bool direction=true; //true-> up, false->down
sandwich 1:0aa1a6ccf5fe 93 float out=0;
sandwich 1:0aa1a6ccf5fe 94 if (curtime>halfperiod) {
sandwich 1:0aa1a6ccf5fe 95 direction=!direction;
sandwich 1:0aa1a6ccf5fe 96 t->reset();
sandwich 1:0aa1a6ccf5fe 97 }
sandwich 1:0aa1a6ccf5fe 98 if (direction==true)
sandwich 2:d95c40ac8a3e 99 out=0.5f+0.5*amp;
sandwich 2:d95c40ac8a3e 100 if (out>1)
sandwich 2:d95c40ac8a3e 101 out=1;
sandwich 1:0aa1a6ccf5fe 102 else if (direction==false)
sandwich 2:d95c40ac8a3e 103 out=0.5f-0.5*amp;
sandwich 2:d95c40ac8a3e 104 if (out<0)
sandwich 2:d95c40ac8a3e 105 out=0;
sandwich 2:d95c40ac8a3e 106 return out;
sandwich 2:d95c40ac8a3e 107 }
sandwich 2:d95c40ac8a3e 108
sandwich 2:d95c40ac8a3e 109 sawGait::sawGait(float freq, float amplitude) : fishgait(), frq(freq), amp(amplitude)
sandwich 2:d95c40ac8a3e 110 {
sandwich 2:d95c40ac8a3e 111 }
sandwich 2:d95c40ac8a3e 112
sandwich 2:d95c40ac8a3e 113 float sawGait::compute()
sandwich 2:d95c40ac8a3e 114 {
sandwich 2:d95c40ac8a3e 115 float period=(1.0f/frq)*1000; //the period in ms
sandwich 2:d95c40ac8a3e 116 int curtime=t->read_ms(); //read time
sandwich 2:d95c40ac8a3e 117 float out=0;
sandwich 2:d95c40ac8a3e 118 if (curtime>period) {
sandwich 2:d95c40ac8a3e 119 t->reset();
sandwich 2:d95c40ac8a3e 120 }
sandwich 2:d95c40ac8a3e 121 out=float(curtime)/period;
sandwich 2:d95c40ac8a3e 122 out-=0.5; //recenter the wave around 0 instead of 0.5
sandwich 2:d95c40ac8a3e 123 out*=amp; //factor in amplitude
sandwich 2:d95c40ac8a3e 124 out+=0.5; //recenter around 0.5
sandwich 2:d95c40ac8a3e 125 if (out>1)
sandwich 2:d95c40ac8a3e 126 out=1;
sandwich 2:d95c40ac8a3e 127 else if (out<0)
sandwich 2:d95c40ac8a3e 128 out=0;
sandwich 1:0aa1a6ccf5fe 129 return out;
sandwich 0:aec7653a5b19 130 }