summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--Makefile2
-rw-r--r--regulator.h68
-rw-r--r--regulator_test.cpp74
-rw-r--r--ringbuf.h34
4 files changed, 163 insertions, 15 deletions
diff --git a/Makefile b/Makefile
index e3d4701..6d85777 100644
--- a/Makefile
+++ b/Makefile
@@ -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;
+ }
+}
diff --git a/ringbuf.h b/ringbuf.h
index 113e5ee..0d11c8d 100644
--- a/ringbuf.h
+++ b/ringbuf.h
@@ -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