regulator + test

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