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;
}
}
|
