Project

General

Profile

Revision 677

Added by James Kong over 13 years ago

encoder: added data array buffer and accumulators

View differences:

branches/encoders/code/projects/libdragonfly/encoders.c
10 10
unsigned short int left_data;
11 11
unsigned short int right_data;
12 12

  
13
RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int);
13
unsigned short int left_data_array[BUFFER_SIZE];
14
unsigned short int right_data_array[BUFFER_SIZE];
15
int left_data_idx;
16
int right_data_idx;
14 17

  
18
int left_dx;
19
int right_dx;
20
long int timecount;
21

  
22
void encoder_recv(char data);
23

  
24
//Helper Function Prototypes
25
inline void left_data_array_put(unsigned short int value);
26
inline unsigned short int left_data_array_top(void);
27
inline unsigned short int left_data_array_prev(void);
28
inline unsigned short int left_data_array_bottom(void);
29

  
30
inline void right_data_array_put(unsigned short int value);
31
inline unsigned short int right_data_array_top(void);
32
inline unsigned short int right_data_array_prev(void);
33
inline unsigned short int right_data_array_bottom(void);
34

  
35
//RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int);
36

  
15 37
void encoders_init(void){
38
	int i;
39

  
16 40
	spi_init(encoder_recv);
17 41
	buf_index = 0;
18 42
	left_data_buf = 0;
19 43
	right_data_buf= 0;
20 44
	left_data = -1;
21 45
	right_data = -1;
22
	RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
46
	//RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
47
	left_data_idx = 0;
48
	right_data_idx = 0;
49
	for(i = 0; i < BUFFER_SIZE; i++) {
50
		left_data_array[i] = 0;
51
	}
52
	for(i = 0; i < BUFFER_SIZE; i++) {
53
		right_data_array[i] = 0;
54
	}
23 55
}
24 56

  
25 57
int encoder_read(char encoder){
......
38 70
	return 0;
39 71
}
40 72

  
73
int encoder_get_dx(char encoder) {
74
	if(encoder==LEFT)
75
		return left_dx;
76
	else if(encoder==RIGHT)
77
		return right_dx;
78
	else return -1;
79
}
80

  
81
void encoder_rst_dx(char encoder) {
82
	if(encoder==LEFT)
83
		left_dx = 0;
84
	else if(encoder==RIGHT)
85
		right_dx = 0;
86
}
87

  
88
int encoder_get_tc(void) {
89
	return timecount;
90
}
91

  
92
void encoder_rst_tc(void) {
93
	timecount = 0;
94
}
95

  
96

  
97

  
41 98
//Full reads occur every 40 microseconds. This function should be called
42 99
//every 8 microseconds.
43 100
void encoder_recv(char data){
101
	short int dx;
102
	
44 103
	if(buf_index < 2)
45 104
		right_data_buf |= (((short)data) << ((1-buf_index)*8)) & (0xFF<<(1-buf_index));
46 105
	
......
78 137
		left_data_buf = 0;
79 138
		right_data_buf = 0;
80 139
	}
140

  
141
	
142
	if(left_data < INVALID) {
143
		//Put new data onto data array
144
		left_data_array_put(left_data);
145

  
146
		//Adjust left accumulator
147
		dx = left_data - left_data_array_prev();
148
		if(dx > 5) { //underflow
149
			left_dx += dx - 1023;
150
		}
151
		else if(dx < -5) { //overflow
152
			left_dx += dx + 1023;
153
		}
154
		else {
155
			left_dx += dx;
156
		}
157
	}
158

  
159
	if(right_data < INVALID) {
160
		//Put new data onto data array
161
		right_data_array_put(right_data);
162

  
163
		//Adjust right accumulator
164
		dx = right_data - right_data_array_prev();
165
		if(dx > 5) { //underflow
166
			right_dx += dx - 1023;
167
		}
168
		else if(dx < -5) { //overflow
169
			right_dx += dx + 1023;
170
		}
171
		else {
172
			right_dx += dx;
173
		}
174
	}
175

  
176
	//Increment timecount accumulator
177
	timecount++;
81 178
}
179

  
180
//Helper Functions
181
inline void left_data_array_put(unsigned short int value) {
182
	if(left_data_idx == BUFFER_SIZE-1)
183
		left_data_idx = 0;
184
	else
185
		left_data_idx++;
186
	left_data_array[left_data_idx] = value;
187
}
188

  
189
inline unsigned short int left_data_array_top(void) {
190
	return left_data_array[left_data_idx];
191
}
192

  
193
inline unsigned short int left_data_array_prev(void) {
194
	if(left_data_idx == 0)
195
		return left_data_array[BUFFER_SIZE-1];
196
	else
197
		return left_data_array[left_data_idx - 1];
198
}
199

  
200
inline unsigned short int left_data_array_bottom(void) {
201
	if(left_data_idx == BUFFER_SIZE-1)
202
		return left_data_array[0];
203
	else
204
		return left_data_array[left_data_idx + 1];
205
}
206

  
207
inline void right_data_array_put(unsigned short int value) {
208
	if(right_data_idx == BUFFER_SIZE-1)
209
		right_data_idx = 0;
210
	else
211
		right_data_idx++;
212
	right_data_array[right_data_idx] = value;
213
}
214

  
215
inline unsigned short int right_data_array_top(void) {
216
	return right_data_array[right_data_idx];
217
}
218

  
219
inline unsigned short int right_data_array_prev(void) {
220
	if(right_data_idx == 0)
221
		return right_data_array[BUFFER_SIZE-1];
222
	else
223
		return right_data_array[right_data_idx - 1];
224
}
225

  
226
inline unsigned short int right_data_array_bottom(void) {
227
	if(right_data_idx == BUFFER_SIZE-1)
228
		return right_data_array[0];
229
	else
230
		return right_data_array[right_data_idx + 1];
231
}
branches/encoders/code/projects/libdragonfly/encoders.h
24 24
int encoder_read(char encoder);
25 25
char encoder_direction(char encoder);
26 26

  
27
int encoder_get_dx(char encoder);
28
void encoder_rst_dx(char encoder);
29
int encoder_get_tc(void);
30
void encoder_rst_tc(void);

Also available in: Unified diff