Lib_Misc
Diff: Unwrapper.cpp
- Revision:
- 0:3312872854c4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Unwrapper.cpp Mon Mar 04 11:03:51 2019 +0000 @@ -0,0 +1,30 @@ +/* +*/ + +#include "Unwrapper.h" +#define pi 3.141592653589793 +using namespace std; + +Unwrapper::Unwrapper(double i2r) +{ + inc2rad = i2r; + last_value = 0; +} + +Unwrapper::~Unwrapper() {} + +void Unwrapper::reset(void) +{ + last_value = 0; +} + +double Unwrapper::doStep(short inc) +{ + long temp = inc; + if((temp - last_value) > 32000) + temp -= 0xFFFF; + else if((temp - last_value) < -32000) + temp += 0xFFFF; + last_value = temp; + return (temp*inc2rad); +} \ No newline at end of file