Ruprecht Altenburger / Library_Misc

2nd try

Dependents:   cuboid_balance

Unwrapper_2pi.cpp@3:29602f4ade5c, 2021-02-25 (annotated)

Committer:
altb2
Date:
Thu Feb 25 20:28:16 2021 +0000
Revision:
3:29602f4ade5c
Parent:
0:72b60c5271cc 
First commit of Mirror actuato, still under construction, pins should be ok, next: check path planner;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:72b60c5271cc 1 /*
altb2 0:72b60c5271cc 2 */
altb2 0:72b60c5271cc 3
altb2 0:72b60c5271cc 4 #include "Unwrapper_2pi.h"
altb2 0:72b60c5271cc 5 #define pi 3.141592653589793
altb2 0:72b60c5271cc 6 using namespace std;
altb2 0:72b60c5271cc 7
altb2 0:72b60c5271cc 8 Unwrapper_2pi::Unwrapper_2pi(void)
altb2 0:72b60c5271cc 9 {
altb2 0:72b60c5271cc 10 last_value = 0.0;
altb2 0:72b60c5271cc 11 turns = 0;
altb2 0:72b60c5271cc 12 }
altb2 0:72b60c5271cc 13
altb2 0:72b60c5271cc 14 Unwrapper_2pi::~Unwrapper_2pi() {}
altb2 0:72b60c5271cc 15
altb2 0:72b60c5271cc 16 void Unwrapper_2pi::reset(void)
altb2 0:72b60c5271cc 17 {
altb2 0:72b60c5271cc 18 last_value = 0.0;
altb2 0:72b60c5271cc 19 turns = 0;
altb2 0:72b60c5271cc 20 }
altb2 0:72b60c5271cc 21
altb2 0:72b60c5271cc 22 float Unwrapper_2pi::doStep(float in)
altb2 0:72b60c5271cc 23 {
altb2 0:72b60c5271cc 24 float temp = in + 2*pi*(float)turns;
altb2 0:72b60c5271cc 25 if((temp - last_value) > pi){
altb2 0:72b60c5271cc 26 temp -= 2*pi;
altb2 0:72b60c5271cc 27 turns--;
altb2 0:72b60c5271cc 28 }
altb2 0:72b60c5271cc 29 else if((temp - last_value) < -pi){
altb2 0:72b60c5271cc 30 temp += 2*pi;
altb2 0:72b60c5271cc 31 turns++;
altb2 0:72b60c5271cc 32 }
altb2 0:72b60c5271cc 33 last_value = temp;
altb2 0:72b60c5271cc 34 return (temp);
altb2 0:72b60c5271cc 35 }