23 lines
418 B
C++
23 lines
418 B
C++
|
#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;
|
||
|
|
|
||
|
|
}
|