summaryrefslogtreecommitdiff
path: root/regulator_test.cpp
blob: 5dfe78bc720639b4e2e236272447e6fedcfb96c5 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
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;
	}
}