diff options
| -rw-r--r-- | Makefile | 2 | ||||
| -rw-r--r-- | regulator.h | 68 | ||||
| -rw-r--r-- | regulator_test.cpp | 74 | ||||
| -rw-r--r-- | ringbuf.h | 34 |
4 files changed, 163 insertions, 15 deletions
@@ -1,5 +1,7 @@ client2: client2.cpp lib.cpp ringbuf.h contrib/OpenHMD/src/.libs/libopenhmd.a g++ -std=c++11 -g client2.cpp lib.cpp -lglfw -lGLEW -lGLU -lGL `pkg-config --libs opencv` -lm -DOHMD_STATIC -Icontrib/OpenHMD/include/ contrib/OpenHMD/src/.libs/libopenhmd.a -lhidapi-libusb -lrt -lpthread -o client2 +regulator_test: regulator_test.cpp + g++ -std=c++11 -g regulator_test.cpp `pkg-config --libs opencv` -lm -o regulator_test client: client.c gcc client.c -lX11 -lXi -lXmu -lglut -lGL -lGLU -lm -o client diff --git a/regulator.h b/regulator.h new file mode 100644 index 0000000..f243848 --- /dev/null +++ b/regulator.h @@ -0,0 +1,68 @@ +#ifndef __REGULATOR_H__ +#define __REGULATOR_H__ +class Regulator +{ + private: + long int lasttime; + bool first_run; + double prop,integ,diff; + double sum, result; + double damped_val; + double damping; + public: + void reset(double p, double i, double d, double damp) + { + prop=p; + integ=i; + diff=d; + damping=damp; + + first_run = true; + sum=0; + result=0.0; + } + + void reset() + { + reset(prop,integ,diff,damping); + } + + Regulator(double p, double i, double d, double damp) + { + reset(p,i,d,damp); + } + + void put(double val, long int time_msec) + { + double deriv; + double last_damped_val; + + + if (first_run) + { + damped_val = val; + last_damped_val = damped_val; + lasttime = time_msec - 1; // prevent divison by zero + first_run = false; + } + else + { + last_damped_val = damped_val; + damped_val = damping * damped_val + (1-damping) * val; + } + + double delta_t = (time_msec - lasttime) / 1000.; + lasttime = time_msec; + + sum += val * delta_t; + deriv = (damped_val - last_damped_val) / delta_t; + + result = prop * val + integ * sum + diff * deriv; + } + + double get() + { + return result; + } +}; +#endif diff --git a/regulator_test.cpp b/regulator_test.cpp new file mode 100644 index 0000000..5dfe78b --- /dev/null +++ b/regulator_test.cpp @@ -0,0 +1,74 @@ +#include <unistd.h> +#include <stdio.h> +#include <stdlib.h> +#include <sys/socket.h> +#include <string.h> +#include <sys/un.h> +#include <time.h> +#include <pthread.h> +#include <semaphore.h> +#include "ringbuf.h" +#include "regulator.h" +#include <opencv2/opencv.hpp> + +using namespace std; +using namespace cv; + +#define PI 3.141592654 +#define SIZE 300 + +int mouse_x=0, mouse_y=0; + +void onMouse(int event, int x, int y, int, void*) +{ + mouse_x = x; + mouse_y = y; +} + +void draw_angle(Mat& m, float angle, Scalar col) +{ + line(m, Point(SIZE/2,SIZE/2), Point(SIZE/2+cos(angle)*SIZE/3, SIZE/2+sin(angle)*SIZE/3), col); +} + +float fixup_range(float a, float low, float upp) +{ + float tot=upp-low; + while (a < low) a+=tot; + while (a>= upp) a-=tot; + return a; +} + + +int main() +{ + Mat visu; + namedWindow("ficken"); + setMouseCallback("ficken", onMouse); + int time_msec=0; + float winkel_ist=0; + Regulator regulator(1.0,0.0,1,0.0); + Ringbuffer cmd_delay_queue(50); // 1 sec delay + Ringbuffer lazyness_ringbuf(2); + + while(waitKey(10)&255 !='x') + { + visu=Mat::zeros(SIZE,SIZE,CV_32FC3); + + float winkel_soll = atan2(mouse_y-SIZE/2, mouse_x-SIZE/2); + draw_angle(visu, winkel_soll, Scalar(0,255,0)); + + regulator.put(fixup_range(winkel_soll-winkel_ist,-PI,PI), time_msec); + cmd_delay_queue.put(regulator.get()); + + + lazyness_ringbuf.put(cmd_delay_queue.front()); + cout << winkel_soll << "\t" << winkel_ist << "\t" << cmd_delay_queue.front() << "\t" << regulator.get() <<endl; + winkel_ist=fixup_range(winkel_ist+lazyness_ringbuf.get()/100., -PI,PI); + + draw_angle(visu, winkel_ist, Scalar(0,0,255)); + + imshow("ficken", visu); + + time_msec+=10; + } +} @@ -41,8 +41,8 @@ class Ringbuffer buf = new double[size]; for (int i=0; i<size; i++) buf[i] = 0; - avg = 0.0; - avg_valid = false; + sum_ = 0.0; + sum_valid = false; } ~Ringbuffer() @@ -50,18 +50,22 @@ class Ringbuffer delete [] buf; } - double get() + double sum() { - if (!avg_valid) + if (!sum_valid) { - avg=0.0; + sum_=0.0; for (int i=0; i<size; i++) - avg += buf[i]; - avg/=size; - avg_valid=true; + sum_ += buf[i]; + sum_valid=true; } - return avg; + return sum_; + } + + double get() + { + return sum()/size; } double front() @@ -73,30 +77,30 @@ class Ringbuffer { buf[idx] = val; idx = (idx+1) % size; - avg_valid = false; + sum_valid = false; } void set(double val) { for (int i=0; i<size; i++) buf[i]=val; - avg = val; - avg_valid=true; + sum_ = size*val; + sum_valid=true; } void add(double val) { for (int i=0; i<size; i++) buf[i]+=val; - avg += val; + sum_ += size*val; } private: double* buf; int idx; int size; - double avg; - bool avg_valid; + double sum_; + bool sum_valid; }; class ModuloRingbuffer |