1 files changed, 74 insertions, 0 deletions
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;
+	}
+}