polymorphic base structures for easily implementing different gaits. Triangle wave is already in there
Dependents: robotic_fish_ver_4_9_pixy
fishgait.cpp
00001 #include "fishgait.h" 00002 00003 fishgait::fishgait() 00004 { 00005 t=new Timer(); 00006 t->start(); 00007 } 00008 00009 fishgait::~fishgait() 00010 { 00011 t->stop(); 00012 delete t; 00013 } 00014 00015 void fishgait::setTimer(Timer* tObject) 00016 { 00017 delete t; //not sure if this is safe 00018 t=NULL; 00019 t=tObject; 00020 } 00021 /* 00022 mixedGait::mixedGait() : count(0) 00023 { 00024 } 00025 00026 void mixedGait::attach(fishgait gait, float weight) 00027 { 00028 gaits[count]=gait; 00029 weights[count]=weight; 00030 ++count; 00031 return; 00032 } 00033 00034 void mixedGait::compute() 00035 { 00036 float out=0; 00037 if (count==0) //derp 00038 return 0.5; 00039 //do the mixing 00040 for (int i=0; i<count; ++i) { 00041 out+=gaits[count].compute()*weights[count]; 00042 } 00043 return out; 00044 } 00045 */ 00046 triangleGait::triangleGait(float freq, float amplitude) : fishgait(), frq(freq), amp(amplitude) 00047 { 00048 } 00049 /* 00050 triangleGait::~triangleGait() 00051 { 00052 } 00053 */ 00054 00055 float triangleGait::compute() 00056 { 00057 float halfperiod=(1.0f/(frq/2.0f))*1000; //the half period in ms 00058 int curtime=t->read_ms(); //read time 00059 static bool direction=true; //true-> up, false->down 00060 float out=0; 00061 if (curtime>halfperiod) { 00062 direction=!direction; 00063 t->reset(); 00064 } 00065 if (direction==true) 00066 out=float(curtime)/halfperiod; 00067 else if (direction==false) 00068 out=1.0f-(float(curtime)/halfperiod); 00069 out-=0.5; //recenter the wave around 0 instead of 0.5 00070 out*=amp; //factor in amplitude 00071 out+=0.5; //recenter around 0.5 00072 if (out>1) 00073 out=1; 00074 else if (out<0) 00075 out=0; 00076 return out; 00077 } 00078 00079 squareGait::squareGait(float freq, float amplitude) : fishgait(), frq(freq), amp(amplitude) 00080 { 00081 } 00082 /* 00083 triangleGait::~triangleGait() 00084 { 00085 } 00086 */ 00087 00088 float squareGait::compute() 00089 { 00090 float halfperiod=(1.0f/(frq/2.0f))*1000; //the half period in ms 00091 int curtime=t->read_ms(); //read time 00092 static bool direction=true; //true-> up, false->down 00093 float out=0; 00094 if (curtime>halfperiod) { 00095 direction=!direction; 00096 t->reset(); 00097 } 00098 if (direction==true) 00099 out=0.5f+0.5*amp; 00100 if (out>1) 00101 out=1; 00102 else if (direction==false) 00103 out=0.5f-0.5*amp; 00104 if (out<0) 00105 out=0; 00106 return out; 00107 } 00108 00109 sawGait::sawGait(float freq, float amplitude) : fishgait(), frq(freq), amp(amplitude) 00110 { 00111 } 00112 00113 float sawGait::compute() 00114 { 00115 float period=(1.0f/frq)*1000; //the period in ms 00116 int curtime=t->read_ms(); //read time 00117 float out=0; 00118 if (curtime>period) { 00119 t->reset(); 00120 } 00121 out=float(curtime)/period; 00122 out-=0.5; //recenter the wave around 0 instead of 0.5 00123 out*=amp; //factor in amplitude 00124 out+=0.5; //recenter around 0.5 00125 if (out>1) 00126 out=1; 00127 else if (out<0) 00128 out=0; 00129 return out; 00130 }
Generated on Tue Jul 12 2022 16:19:49 by 1.7.2