master-2018/camera.c at master · Compotes/master-2018 · GitHub

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
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
#include "camera.h"

#define MAIN 2
#define READ 1
#define CAMERA_SERIAL &SD4

thread_t *camera_read_thread;
thread_t *camera_process_thread;

int16_t camera_read_values[NUMBER_OF_VALUES];
int16_t camera_actual_values[NUMBER_OF_VALUES];
int16_t camera_values[NUMBER_OF_VALUES];

char current_value;

THD_WORKING_AREA(waCameraReadThread, 128);
THD_FUNCTION(CameraReadThread, arg) {
	(void)arg;

	int8_t i;
	int16_t goal_degree = 420;
	int16_t goal_distance = 420;
	int16_t ball_degree = 420;
	int16_t ball_distance = 420;
	camera_read_values[4] = 0;

	while (1) {
		for(i = 0; i < 2; i++){
			current_value = sdGet(CAMERA_SERIAL);
			if(current_value == 'x') {
				goal_degree = process_value();
			} else if (current_value == 'y') {
				goal_distance = process_value();
			} else if (current_value == 'z') {
				ball_degree = process_value();
			} else if (current_value == 'i') {
				ball_distance = process_value();
			} else if (current_value == 'p') {
				camera_read_values[4] = 1;
			} else if (current_value == 's') {
				camera_read_values[4] = 0;
			} else if (current_value == 'k') {
				kick();
			} else if (current_value == 'a') {
				camera_read_values[5] = 1;
			} else if (current_value == 'b') {
				camera_read_values[5] = 0;
			}
		}

		camera_read_values[0] = goal_degree;
		camera_read_values[1] = goal_distance;
		camera_read_values[2] = ball_degree;
		camera_read_values[3] = ball_distance;

		chMsgSend(camera_process_thread, (msg_t)READ);
	}
}

THD_WORKING_AREA(waCameraProcessThread, 128);
THD_FUNCTION(CameraProcessThread, arg) {
	(void)arg;

	int8_t i;

	thread_t *master_thread;
	msg_t msg;

	while(1) {
		master_thread = chMsgWait();
		msg = chMsgGet(master_thread);
		if(msg == READ) {
			for(i = 0; i < NUMBER_OF_VALUES; i++) {
				camera_actual_values[i] = camera_read_values[i];
			}
		} else {
			for(i = 0; i < NUMBER_OF_VALUES; i++) {
				camera_values[i] = camera_actual_values[i];
			}
		}
		chMsgRelease(master_thread, MSG_OK);
	}
}

void get_camera_values(void) {
	chMsgSend(camera_process_thread, (msg_t)MAIN);
}

int16_t get_goal_degree(void) {
	return camera_values[0];
}

int16_t get_goal_distance(void) {
	return camera_values[1];
}

int16_t get_ball_degree(void) {
	return camera_values[2];
}

int16_t get_ball_distance(void) {
	return camera_values[3];
}

int16_t get_start(void) {
	return camera_values[4];
}

int16_t get_shooting_event(void) {
	return camera_values[5];
}

int16_t process_value(void) {
	int16_t val = 0;

	current_value = sdGet(&SD4);
	while(current_value != '+' && current_value != '-') {
		val += current_value - '0';
		val *= 10;
		current_value = sdGet(&SD4);
	}
	val /= 10;
	if(current_value == '-') return -val;
	return val;
}

void camera_init(void) {
	camera_read_thread = chThdCreateStatic(waCameraReadThread, sizeof(waCameraReadThread), NORMALPRIO, CameraReadThread, NULL);
	camera_process_thread = chThdCreateStatic(waCameraProcessThread, sizeof(waCameraProcessThread), NORMALPRIO, CameraProcessThread, NULL);
}