HAHA! commit
This commit is contained in:
gauthiier
Executable
+210
@@ -0,0 +1,210 @@
|
||||
|
||||
/*
|
||||
777$7II7III??+,.......:+~=7$$Z$?$OOZOZ77?$$I+??I$ZOD8DD8DD888O$7$77$Z$$$ZZZZZOOO
|
||||
$$$777$$77II??~.,..,.=?777$ZZ==$OZZ$I?=+?Z+==~=7Z7Z$OO8888DDDD8ZOZ$$$7Z$ZOZ$ZZOO
|
||||
$$$7ZZ$Z$$77II=~:.,.:??+IZZZ+?O$ZI+?+:~+7I~~,:~?Z$88O88D88DDDDD8OZOZ$$$ZZZZZOOZZ
|
||||
$$$$ZZ$$$$777I??:::,:7ZZ8OZ$$$7?=::,:~??~:,.....7$8$IZDDD8DDDNDDOZZO$$ZZZZZOZOOO
|
||||
$$$77$7$7III$II+==+$$$OOO8OZ?=:......~. ........,?ZOI$88DOD8DDN8D888$7$ZZ$$Z$$ZO
|
||||
$$7$$$77I77$ZZ$7,:Z$OZOOOOOI. ...,~:.....:~~+:===$Z$7OOD8DDD88O8D888O$7$Z$$$$OZ
|
||||
$$77$$77I$$ZOZI,.7Z88O88O8?. .,:~~::,,.,,,+?$ZZZZZ8OZZ$8$8NDDDD88ODO8ZZ$$$OZZZOO
|
||||
Z$Z$$$77$Z$$O$~.~?$ZOOOOOZ, ,?7$7II+:..,~+$III777I77I$O8OD8DD8D8DO8D88$77$$$ZZZZ
|
||||
OOOOZc$ZOZOZZ?..+??IZO8NO$I?===+=~~~,...,~II???+~:+~:+ZO8D88DOODDDDD8D88OZ77$ZZZ
|
||||
ZOZOOZZZZZOZ$II.~?I$ZZO8ZO~. ..~?+~~.. .,?$I$ZO888O?~$Z88D888O8888DONDD88Z7777$
|
||||
OZZ8O8OZZOZZZZ7I7I$ZZ7$OZ7:.=7ZDO7?~.. ,::+O7$O8O78O7IOD88N8D8DD88DD8OOOO7$$7Z
|
||||
OOOZ88888OOOOZZI~?I$Z$OZI+?77,?$I?:.. .,:,,~,,~+++~+I$ZZ8DDD8DNDD8DDOOOZ7?$$7$
|
||||
OOOZO88OO8OZ7=:,.?7IZZOZI,~=~. ... . ..,,.. ...=:..~I$Z8DDDD8D8DDOD88a7II777$
|
||||
$OOOOOOO8O+.,.:++:++$ZOO7 .,. .,,,.... ..:~.:$Z8DD8DDD88DDD8O$?????I7
|
||||
OO8O8O8OZ$.,:?=..::?IZZ8Z .. ..,.,=,.. ..:I??7O8DD88D888D8OOI?II77II
|
||||
OOOO8OZZ7=,~7?~,,,~~?$7OZ,. .,:~=:,.,:, ..:=+:~788DDD88Z888OZ$+I?III$7
|
||||
ZOZZO$$$ZZZZ$I:~:~,,7=77$~:.. .:$$==IOO7=,.,....,:,:=+$D8DD8888D887II?7777$$
|
||||
ZOOZZZ$$ZZZZ7+::=:::::II~.:.. ..=+?+IIII:...,,..,,,:==$DDD88888DD8$ZZ$$ZOOOO
|
||||
7ZO$n7I$$$Z$7I~,,:,,.,=?:,,.. . ,:::~~I?:,......,,:,:=ZO888D88D8DO88O8888888
|
||||
77777$77$$7I?~,.. .,,,=+~.. ~+=:~+:+II+=~:,:..,,::~?ZODO888D88DD8DDDD8D88N
|
||||
?77???I?II?~:,.....=.:?+~. ., .:?7I:::~::=+~~...,:~=?$OOOZ888OO8OO8888D8D8D
|
||||
7II??I+++~:,.. .,==$7+,. .IZ7~~~~,~:++=:..::~++7ZZODDD8D888a8OOD88D88
|
||||
I???+~:,,,... .=:=~I$$+, .$7?+++++=+==+~,,:==+?78DDDDDDDO88OZOO88O888
|
||||
??+::,,... ... .78Z7IZO=...... ,77:..:::.,~?+~::=++~?$8DDDN8D8O88O88OOOOO88
|
||||
~=~:,,,.,.... .:OO$Z$7... ?$~ .......,:=~,:++==?$88D88D8ZOZOOZOOOO8DOO
|
||||
:~=+:,,.. . ..,?$$+.. =...::,... ,~::~?===+OD88888OZOOOZO$ZOOZZZ8
|
||||
,~++++==.. ....~ZZO~. .:~=~,,...:=~~$7?+?$DD8D8OOZZOZOZOOOO8O8O8
|
||||
.:=+==++,. ...,.:+8DZ...I$7:.....,~=:,....+~?I7=?=$O8DDOOZ88OOO8OOOOOZZO88
|
||||
,:~===?d, .:,:?OD8O...ZOO$$?I,...=~:,,,,~===?+?+77Z8OZ$Z$OOZZOOO8OOOOO8O8
|
||||
,:~=~~=+.. ..=Z8888$.+OZ8OOZ8O7,.::=:~~=~:+????I$$?7$$ZZZOOOOOOOOO8O8OOOOO
|
||||
,,:~~~+=.. ~ZO87I7Z,:7OOZOO?OD$~,:~:+?+==+?+=+77$?IOOZZZOOOOO8OOOOOOOOO888
|
||||
..,,~=+~.. . =$OO:..+?.IZOOZOZ$OZ?=~=~=++=+:?III7$7I=7DD8ZZOO8OO88OO8ZOZ$ZOOO
|
||||
..... .... . ,~?7OZ7..:~I+ZZOO$OOOZZ$+7++?+??I777$$Z7I?~Z8D8OO$Z888O8888ZOZOZOO8
|
||||
,.....,~~==:=?$ZOZ7 .,~7IZZOZZZZOOOZO$I$Z$$Z$Z$ZZ7$I+==~DDDN8DDO$O88D8DDZO8OZ888
|
||||
=+=:~::=~~:+$ZZZ$+...I77$OZ$$IIZ$?$Z7=======~=++??==,::=D8D88D8D8ZOODOOOZZ8ZOaOO
|
||||
*/
|
||||
|
||||
/*
|
||||
modified version of:
|
||||
*/
|
||||
|
||||
/*
|
||||
Copyright (c) 2007 Gerhard Reitmayr <gerhard.reitmayr@gmail.com>
|
||||
Permission to use, copy, modify, and distribute this software for any
|
||||
purpose with or without fee is hereby granted, provided that the above
|
||||
copyright notice and this permission notice appear in all copies.
|
||||
THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
|
||||
WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
|
||||
MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
|
||||
ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
|
||||
WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
|
||||
ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
|
||||
OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
|
||||
*/
|
||||
|
||||
#ifndef _FSM_H
|
||||
#define _FSM_H
|
||||
|
||||
/// list of ints as recursive type
|
||||
template <int HEAD, class TAIL> struct IntList {
|
||||
enum { head = HEAD };
|
||||
typedef TAIL tail;
|
||||
};
|
||||
|
||||
/// end of list marker type
|
||||
struct IntListEnd {};
|
||||
|
||||
/// switch statement implementation through iterating over the type list and
|
||||
/// comparing it to a given state
|
||||
template <class STATELIST>
|
||||
struct SwitchTemplate {
|
||||
template <class CONTEXT>
|
||||
static typename CONTEXT::ReturnType work( int state, CONTEXT & context){
|
||||
return ((STATELIST::head == state) ?
|
||||
context.template operator()<STATELIST::head>() :
|
||||
SwitchTemplate<typename STATELIST::tail>::work( state, context ));
|
||||
}
|
||||
};
|
||||
|
||||
/// end of list marker specialization
|
||||
template <>
|
||||
struct SwitchTemplate<IntListEnd> {
|
||||
template <class CONTEXT>
|
||||
static typename CONTEXT::ReturnType work( int state, CONTEXT & context){
|
||||
return typename CONTEXT::ReturnType();
|
||||
}
|
||||
};
|
||||
|
||||
/// The actual state machine implementation
|
||||
/// sub types are functors passed to the Worker class to execute the
|
||||
/// right template specialization of the underlying member function of the
|
||||
/// context object.
|
||||
template <class CONTEXT, class STATELIST = typename CONTEXT::StateList>
|
||||
struct StateMachine {
|
||||
|
||||
CONTEXT & context;
|
||||
int state;
|
||||
|
||||
template <class RET, class DATA> struct CallEvent {
|
||||
typedef RET ReturnType;
|
||||
CONTEXT & context;
|
||||
DATA & data;
|
||||
|
||||
template <int STATE> RET operator()(){
|
||||
return context.template event<STATE>(data);
|
||||
}
|
||||
};
|
||||
|
||||
template <class RET, class DATA> struct CallEventConst {
|
||||
typedef RET ReturnType;
|
||||
CONTEXT & context;
|
||||
const DATA & data;
|
||||
|
||||
template <int STATE> RET operator()(){
|
||||
return context.template event<STATE>(data);
|
||||
}
|
||||
};
|
||||
|
||||
template <class RET> struct CallEventNoData {
|
||||
typedef RET ReturnType;
|
||||
CONTEXT & context;
|
||||
|
||||
template <int STATE> RET operator()(){
|
||||
return context.template event<STATE>();
|
||||
}
|
||||
};
|
||||
|
||||
struct CallEnter {
|
||||
typedef void ReturnType;
|
||||
CONTEXT & context;
|
||||
|
||||
template <int STATE> ReturnType operator()(){
|
||||
return context.template enter<STATE>();
|
||||
}
|
||||
};
|
||||
|
||||
struct CallExit {
|
||||
typedef void ReturnType;
|
||||
CONTEXT & context;
|
||||
|
||||
template <int STATE> ReturnType operator()(){
|
||||
return context.template exit<STATE>();
|
||||
}
|
||||
};
|
||||
|
||||
struct CallTick {
|
||||
typedef void ReturnType;
|
||||
CONTEXT & context;
|
||||
|
||||
template <int STATE> ReturnType operator()(){
|
||||
return context.template tick<STATE>();
|
||||
}
|
||||
};
|
||||
|
||||
StateMachine( CONTEXT & c ) : context(c), state(STATELIST::head) {
|
||||
CallEnter cee = {context};
|
||||
SwitchTemplate<STATELIST>::work(state, cee);
|
||||
}
|
||||
|
||||
void changeState(const int newstate){
|
||||
CallExit cl = {context};
|
||||
SwitchTemplate<STATELIST>::work(state, cl);
|
||||
state = newstate;
|
||||
CallEnter cee = {context};
|
||||
SwitchTemplate<STATELIST>::work(state, cee);
|
||||
}
|
||||
|
||||
void tick() {
|
||||
CallTick ct = {context};
|
||||
SwitchTemplate<STATELIST>::work(state, ct);
|
||||
}
|
||||
|
||||
void work(){
|
||||
CallEventNoData<int> ce = {context};
|
||||
int newstate = SwitchTemplate<STATELIST>::work(state, ce);
|
||||
if(newstate != state)
|
||||
changeState(newstate);
|
||||
}
|
||||
|
||||
template <class EVENT>
|
||||
void work( const EVENT & ev ){
|
||||
CallEventConst<int, EVENT> ce = {context, ev};
|
||||
int newstate = SwitchTemplate<STATELIST>::work(state, ce);
|
||||
if(newstate != state)
|
||||
changeState(newstate);
|
||||
}
|
||||
|
||||
template <class EVENT>
|
||||
void work( EVENT & ev ){
|
||||
CallEvent<int, EVENT> ce = {context, ev};
|
||||
int newstate = SwitchTemplate<STATELIST>::work(state, ce);
|
||||
if(newstate != state)
|
||||
changeState(newstate);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
/// macros for simple state list definition
|
||||
#define LIST1(a) IntList<a,IntListEnd>
|
||||
#define LIST2(a,b) IntList<a,LIST1(b) >
|
||||
#define LIST3(a,b,c) IntList<a,LIST2(b,c) >
|
||||
#define LIST4(a,b,c,d) IntList<a,LIST3(b,c,d) >
|
||||
#define LIST5(a,b,c,d,e) IntList<a,LIST4(b,c,d,e) >
|
||||
#define LIST6(a,b,c,d,e,f) IntList<a,LIST5(b,c,d,e,f) >
|
||||
#define LIST7(a,b,c,d,e,f,g) IntList<a,LIST6(b,c,d,e,f,g) >
|
||||
#define LIST8(a,b,c,d,e,f,g,h) IntList<a,LIST7(b,c,d,e,f,g,h) >
|
||||
|
||||
#endif // _FSM_H
|
||||
Executable
+59
@@ -0,0 +1,59 @@
|
||||
/*
|
||||
777$7II7III??+,.......:+~=7$$Z$?$OOZOZ77?$$I+??I$ZOD8DD8DD888O$7$77$Z$$$ZZZZZOOO
|
||||
$$$777$$77II??~.,..,.=?777$ZZ==$OZZ$I?=+?Z+==~=7Z7Z$OO8888DDDD8ZOZ$$$7Z$ZOZ$ZZOO
|
||||
$$$7ZZ$Z$$77II=~:.,.:??+IZZZ+?O$ZI+?+:~+7I~~,:~?Z$88O88D88DDDDD8OZOZ$$$ZZZZZOOZZ
|
||||
$$$$ZZ$$$$777I??:::,:7ZZ8OZ$$$7?=::,:~??~:,.....7$8$IZDDD8DDDNDDOZZO$$ZZZZZOZOOO
|
||||
$$$77$7$7III$II+==+$$$OOO8OZ?=:......~. ........,?ZOI$88DOD8DDN8D888$7$ZZ$$Z$$ZO
|
||||
$$7$$$77I77$ZZ$7,:Z$OZOOOOOI. ...,~:.....:~~+:===$Z$7OOD8DDD88O8D888O$7$Z$$$$OZ
|
||||
$$77$$77I$$ZOZI,.7Z88O88O8?. .,:~~::,,.,,,+?$ZZZZZ8OZZ$8$8NDDDD88ODO8ZZ$$$OZZZOO
|
||||
Z$Z$$$77$Z$$O$~.~?$ZOOOOOZ, ,?7$7II+:..,~+$III777I77I$O8OD8DD8D8DO8D88$77$$$ZZZZ
|
||||
OOOOZc$ZOZOZZ?..+??IZO8NO$I?===+=~~~,...,~II???+~:+~:+ZO8D88DOODDDDD8D88OZ77$ZZZ
|
||||
ZOZOOZZZZZOZ$II.~?I$ZZO8ZO~. ..~?+~~.. .,?$I$ZO888O?~$Z88D888O8888DONDD88Z7777$
|
||||
OZZ8O8OZZOZZZZ7I7I$ZZ7$OZ7:.=7ZDO7?~.. ,::+O7$O8O78O7IOD88N8D8DD88DD8OOOO7$$7Z
|
||||
OOOZ88888OOOOZZI~?I$Z$OZI+?77,?$I?:.. .,:,,~,,~+++~+I$ZZ8DDD8DNDD8DDOOOZ7?$$7$
|
||||
OOOZO88OO8OZ7=:,.?7IZZOZI,~=~. ... . ..,,.. ...=:..~I$Z8DDDD8D8DDOD88a7II777$
|
||||
$OOOOOOO8O+.,.:++:++$ZOO7 .,. .,,,.... ..:~.:$Z8DD8DDD88DDD8O$?????I7
|
||||
OO8O8O8OZ$.,:?=..::?IZZ8Z .. ..,.,=,.. ..:I??7O8DD88D888D8OOI?II77II
|
||||
OOOO8OZZ7=,~7?~,,,~~?$7OZ,. .,:~=:,.,:, ..:=+:~788DDD88Z888OZ$+I?III$7
|
||||
ZOZZO$$$ZZZZ$I:~:~,,7=77$~:.. .:$$==IOO7=,.,....,:,:=+$D8DD8888D887II?7777$$
|
||||
ZOOZZZ$$ZZZZ7+::=:::::II~.:.. ..=+?+IIII:...,,..,,,:==$DDD88888DD8$ZZ$$ZOOOO
|
||||
7ZO$n7I$$$Z$7I~,,:,,.,=?:,,.. . ,:::~~I?:,......,,:,:=ZO888D88D8DO88O8888888
|
||||
77777$77$$7I?~,.. .,,,=+~.. ~+=:~+:+II+=~:,:..,,::~?ZODO888D88DD8DDDD8D88N
|
||||
?77???I?II?~:,.....=.:?+~. ., .:?7I:::~::=+~~...,:~=?$OOOZ888OO8OO8888D8D8D
|
||||
7II??I+++~:,.. .,==$7+,. .IZ7~~~~,~:++=:..::~++7ZZODDD8D888a8OOD88D88
|
||||
I???+~:,,,... .=:=~I$$+, .$7?+++++=+==+~,,:==+?78DDDDDDDO88OZOO88O888
|
||||
??+::,,... ... .78Z7IZO=...... ,77:..:::.,~?+~::=++~?$8DDDN8D8O88O88OOOOO88
|
||||
~=~:,,,.,.... .:OO$Z$7... ?$~ .......,:=~,:++==?$88D88D8ZOZOOZOOOO8DOO
|
||||
:~=+:,,.. . ..,?$$+.. =...::,... ,~::~?===+OD88888OZOOOZO$ZOOZZZ8
|
||||
,~++++==.. ....~ZZO~. .:~=~,,...:=~~$7?+?$DD8D8OOZZOZOZOOOO8O8O8
|
||||
.:=+==++,. ...,.:+8DZ...I$7:.....,~=:,....+~?I7=?=$O8DDOOZ88OOO8OOOOOZZO88
|
||||
,:~===?d, .:,:?OD8O...ZOO$$?I,...=~:,,,,~===?+?+77Z8OZ$Z$OOZZOOO8OOOOO8O8
|
||||
,:~=~~=+.. ..=Z8888$.+OZ8OOZ8O7,.::=:~~=~:+????I$$?7$$ZZZOOOOOOOOO8O8OOOOO
|
||||
,,:~~~+=.. ~ZO87I7Z,:7OOZOO?OD$~,:~:+?+==+?+=+77$?IOOZZZOOOOO8OOOOOOOOO888
|
||||
..,,~=+~.. . =$OO:..+?.IZOOZOZ$OZ?=~=~=++=+:?III7$7I=7DD8ZZOO8OO88OO8ZOZ$ZOOO
|
||||
..... .... . ,~?7OZ7..:~I+ZZOO$OOOZZ$+7++?+??I777$$Z7I?~Z8D8OO$Z888O8888ZOZOZOO8
|
||||
,.....,~~==:=?$ZOZ7 .,~7IZZOZZZZOOOZO$I$Z$$Z$Z$ZZ7$I+==~DDDN8DDO$O88D8DDZO8OZ888
|
||||
=+=:~::=~~:+$ZZZ$+...I77$OZ$$IIZ$?$Z7=======~=++??==,::=D8D88D8D8ZOODOOOZZ8ZOaOO
|
||||
*/
|
||||
|
||||
#ifndef _MS_H
|
||||
#define _MS_H
|
||||
|
||||
#include "FSM.h"
|
||||
|
||||
#define machine_template template <>
|
||||
|
||||
struct Event { int state; };
|
||||
|
||||
class MachineStates {
|
||||
public:
|
||||
// typedef LIST3(AAAA, BBBB, CCCC) StateList;
|
||||
template <int> int event(Event & ev) { Serial.println("undefined event -- event"); return ev.state; }
|
||||
template <int> int event() { Serial.println("undefined event"); return 0; }
|
||||
template <int> void enter() { Serial.println("undefined enter"); }
|
||||
template <int> void exit() { Serial.println("undefined leave"); };
|
||||
template <int> void tick() { Serial.println("undefined tick");}
|
||||
};
|
||||
|
||||
|
||||
#endif // _MS_H
|
||||
Executable
+16
@@ -0,0 +1,16 @@
|
||||
#pragma once
|
||||
|
||||
class ExpMovAvg {
|
||||
public:
|
||||
ExpMovAvg(int* in_i, int* out_o, float lambda = 0.5) : _in_ptr(in_i), _out_ptr(out_o), _lambda(lambda) {;}
|
||||
|
||||
inline void calculate() {
|
||||
_ema = _lambda * (float)(*_in_ptr) + (1 - _lambda) * _ema;
|
||||
*_out_ptr = _ema;
|
||||
}
|
||||
|
||||
int *_in_ptr, *_out_ptr;
|
||||
int _ema;
|
||||
float _lambda;
|
||||
|
||||
};
|
||||
Executable
+55
@@ -0,0 +1,55 @@
|
||||
#include "MinMaxLerp.h"
|
||||
|
||||
#include "Arduino.h"
|
||||
|
||||
MinMaxLerp::MinMaxLerp(int* in_i, int base_min, int base_max, long update_interval_ms, int nbr_steps) :
|
||||
_in_ptr(in_i), _base_min(base_min), _base_max(base_max), _cur_min(base_min), _cur_max(base_max),
|
||||
_tick_min(0), _tick_max(0), _update_interval(update_interval_ms)
|
||||
{
|
||||
_tick_time = (float)update_interval_ms / (float) nbr_steps;
|
||||
}
|
||||
|
||||
void MinMaxLerp::lerp_min_max() {
|
||||
|
||||
static float m_min = 1;
|
||||
static float m_max = 1;
|
||||
static int t_min = 0;
|
||||
static int t_max = 0;
|
||||
|
||||
int _ema = *_in_ptr;
|
||||
|
||||
if(_ema < _cur_min) {
|
||||
_cur_min = _ema;
|
||||
_min = _ema;
|
||||
_tick_min = millis();
|
||||
t_min = millis();
|
||||
|
||||
// calculate new slope
|
||||
int dy = _base_min - _min;
|
||||
m_min = (float) dy / (float)_update_interval; // linear
|
||||
} else {
|
||||
long t = millis() - _tick_min;
|
||||
if(t < _update_interval && (t_min - millis()) > _tick_time) {
|
||||
t_min = millis();
|
||||
_cur_min = _min + m_min * t;
|
||||
}
|
||||
}
|
||||
|
||||
if(_ema > _cur_max) {
|
||||
_cur_max = _ema;
|
||||
_max = _ema;
|
||||
_tick_max = millis();
|
||||
t_max = millis();
|
||||
|
||||
// calculate new slope
|
||||
int dy = _base_max - _max;
|
||||
m_max = (float) dy / (float)_update_interval; // linear
|
||||
} else {
|
||||
long t = millis() - _tick_max;
|
||||
if(t < _update_interval && (t_max - millis()) > _tick_time) {
|
||||
t_max = millis();
|
||||
_cur_max = _max + m_max * t;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
Executable
+26
@@ -0,0 +1,26 @@
|
||||
#pragma once
|
||||
|
||||
class MinMaxLerp {
|
||||
public:
|
||||
MinMaxLerp(int* in_i, int base_min, int base_max, long update_interval_ms = 1000, int nbr_steps = 100);
|
||||
|
||||
void lerp_min_max();
|
||||
|
||||
inline int lmin() { return _cur_min; }
|
||||
inline int lmax() { return _cur_max; }
|
||||
|
||||
protected:
|
||||
|
||||
int _base_min;
|
||||
int _base_max;
|
||||
int _min;
|
||||
int _max;
|
||||
int _cur_min;
|
||||
int _cur_max;
|
||||
|
||||
long _tick_min, _tick_max, _tick_time;
|
||||
long _update_interval;
|
||||
|
||||
int* _in_ptr;
|
||||
|
||||
};
|
||||
Executable
+23
@@ -0,0 +1,23 @@
|
||||
#include "SpringMassDamper.h"
|
||||
|
||||
#include "Arduino.h"
|
||||
|
||||
SpringMassDamper::SpringMassDamper(float m, float k, float d, float* out) :
|
||||
_m(m), _k(k), _d(d), _out(out) {}
|
||||
|
||||
float SpringMassDamper::position(int x) {
|
||||
|
||||
float dt = (millis() - _tick) / 1000.0f;
|
||||
|
||||
F = _k * (x - X) - (_d * V);
|
||||
V += (F / _m) * dt;
|
||||
X += V * dt;
|
||||
|
||||
_tick = millis();
|
||||
|
||||
if(_out != NULL)
|
||||
*_out = X;
|
||||
|
||||
return X;
|
||||
|
||||
}
|
||||
Executable
+16
@@ -0,0 +1,16 @@
|
||||
#include "Arduino.h"
|
||||
|
||||
#pragma once
|
||||
|
||||
class SpringMassDamper {
|
||||
public:
|
||||
SpringMassDamper(float m, float k, float d, float* out = NULL);
|
||||
|
||||
float position(int x);
|
||||
|
||||
float X, V, F;
|
||||
float _m, _k, _d;
|
||||
long _tick;
|
||||
float* _out;
|
||||
|
||||
};
|
||||
Reference in New Issue
Block a user