Project

General

Profile

Revision 809

Recompiled the library.

View differences:

branches/encoders/code/lib/src/libdragonfly/encoders.c
22 22
int left_v;
23 23
int right_v;
24 24

  
25
volatile int recv_count;
26 25

  
27 26
volatile short int data_ready;
28 27

  
......
45 44

  
46 45
void encoder_recv_complete(){
47 46

  
48
int dx;
49

  
50 47
   //  usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n");
51 48
   //   usb_puts("\r\n");
52 49
  data_ready++;
53 50
  
54
  delay_ms(ENCODER_DELAY);
51
  //delay_ms(ENCODER_DELAY);
55 52

  
56 53
   spi_transfer(5);
57 54
}
......
64 61
  usb_puts(" ");
65 62
}
66 63

  
64
/**
65
 * Initializes the encoders variables
66
 **/
67 67
void encoders_init(void){
68 68
	int i;
69 69
	
......
76 76
	left_data = -1;
77 77
	right_data = -1;
78 78
	
79
	recv_count=0;
80 79
	
81 80
	left_v=0;
82 81
	right_v=0;
......
93 92
	spi_transfer(5);
94 93
}
95 94

  
95
/**
96
 * Returns the specified encoders value
97
 *
98
 * @param encoder this is the encoder that you want to read. Valid arguments
99
 *          are LEFT and RIGHT
100
 *
101
 * @return the value of the specified encoder
102
 **/
96 103
int encoder_read(char encoder){
97 104
	if(encoder==LEFT)
98 105
		return left_data;
......
108 115
char encoder_direction(char encoder){
109 116
	return 0;
110 117
}
111

  
118
/**
119
 * Gets the velocity of the specified encoder.
120
 *
121
 * @param encoder the encoder that you want to read, use LEFT or RIGHT
122
 *
123
 * @return The velocity of the specified encoder.
124
 **/
112 125
int encoder_get_dx(char encoder) {
113 126
	if(encoder==LEFT)
114 127
		return left_dx;
......
117 130
	else return -1;
118 131
}
119 132

  
133
/**
134
 * Resets the value of the velocity global variable for the specified
135
 *  encoder.
136
 *
137
 * @param encoder the encoder that you want to modify
138
 **/
120 139
void encoder_rst_dx(char encoder) {
121 140
	if(encoder==LEFT)
122 141
		left_dx = 0;
......
124 143
		right_dx = 0;
125 144
}
126 145

  
146
/**
147
 * Returns the current time count for the encoders.
148
 **/
127 149
int encoder_get_tc(void) {
128 150
	return timecount;
129 151
}
130 152

  
153
/**
154
 * Resets the time count for the encoders.
155
 **/
131 156
void encoder_rst_tc(void) {
132 157
	timecount = 0;
133 158
}
......
172 197
//Full reads occur every 40 microseconds. This function should be called
173 198
//every 8 microseconds.
174 199
void encoder_recv(char data){
175
	//usb_puti(buf_index);
176 200
	short int dx;
177 201
	
178
	recv_count++;
179
	//if(++recv_count==ENCODER_DELAY){
180
			
181
		recv_count=0;
202
	//Parse the encoder data, comes in over 5 bytes 16 bits per encoder,
203
	// second is offset by 1 bit.
204
	switch(buf_index){	
205
	case 0: 
206
		right_data_buf |= ((short)data)<<8 & 0xff00;
207
		break;
208
	case 1:
209
		right_data_buf |= ((short)data) & 0xff;
210
		break;
211
	case 2:
212
		left_data_buf |= (((short)data) << 9) & (0x7F << 9);
213
		break;
214
	case 3:
215
		left_data_buf |= (((short)data) << 1) & (0xFF<<1);
216
		break;
217
	case 4: left_data_buf |= (((short)data)>>7) & 0x1;
218
	}	
182 219
		
183
		if(buf_index == 0)
184
		  right_data_buf |= ((short)data)<<8 & 0xff00;
220
	buf_index = (buf_index + 1) % 5;
185 221

  
186
		else if (buf_index == 1)
187
		  right_data_buf |= ((short)data) & 0xff;
188
		
189
		else if (buf_index == 2)
190
			left_data_buf |= (((short)data) << 9) & (0x7F << 9);
191
		
192
		else if (buf_index == 3)
193
			left_data_buf |= (((short)data) << 1) & (0xFF<<1);
194
		
195
		else if (buf_index == 4)
196
			left_data_buf |= (((short)data)>>7) & 0x1;
197

  
198
		
199
		buf_index = (buf_index + 1) % 5;
200

  
201
		if(buf_index==0) {
202
		  if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF)){
203
				left_data = INVALID;
204
		  }
222
	if(buf_index==0) {
223
		  
224
		if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF))  left_data = INVALID;
225
		else if(((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)) left_data = MAGNET_FAILURE;
226
		else left_data = (left_data_buf>>5) & 1023;
205 227
			
206
		  else if(((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)){
207
			left_data = MAGNET_FAILURE;
208
		  }
209
		  else{
210
			left_data = (left_data_buf>>5) & 1023;
211
		  }
228
		if(right_data_buf & (COF | LIN) || !(right_data_buf & OCF)) right_data = INVALID;
229
		else if ( ((right_data_buf & MagINCn) > 0)  && ((right_data_buf & MagDECn) > 0)) right_data = MAGNET_FAILURE;
230
		else right_data = (right_data_buf>>5) & 1023;
212 231
			
213
		  if(right_data_buf & (COF | LIN) || !(right_data_buf & OCF)){
214
			right_data = INVALID;
215
		  }
216
		  else if ( ((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)){
217
			left_data = MAGNET_FAILURE;
218
		  }
219
		  else{
220
			right_data = (right_data_buf>>5) & 1023;
221
		  }
222
		
232
  		left_data_buf = 0;
233
		right_data_buf = 0;
223 234

  
224
			//if(left_data!=INVALID || right_data!=INVALID){
225
			//  usb_puts("{");usb_puti(left_data);usb_puts(",");usb_puti(right_data);usb_puts("}\r\n");
226
			//}
227
			
228
			  
229
  
230
  			left_data_buf = 0;
231
			right_data_buf = 0;
232

  
233 235
		
234
		if(left_data < INVALID) {
235
			//Put new data onto data array
236
		if(left_data != INVALID && left_data != MAGNET_FAILURE) {
236 237
			left_data_array_put(left_data);
237 238

  
238 239
			//Adjust left accumulator
239
			//dx = left_data - left_data_array_prev();
240 240
			dx = left_data_array_prev() - left_data;
241 241
			
242 242
			//Adjust velocity: save last dx
243 243
			left_v = left_dx;
244 244

  
245
			/*left dx was negative on robot 7, could just be that
246
			  the encoder is backwards, so this may need to change
247
			  back
248
			*/
245
			if(left_data_array_prev()==0)  dx=0;
249 246

  
250

  
251
			if(left_data_array_prev()==0)
252
			  dx=0;
253

  
254
			if(dx > 5) { //underflow
255
				left_dx += dx - 1023;
256
			}
257
			else if(dx < -5) { //overflow
258
				left_dx += dx + 1023;
259
			}
260
			else {
261
				left_dx += dx;
262
			}
247
			if(dx > 512) left_dx += dx - 1023; //Underflow
248
			else if(dx < -512) left_dx += dx + 1023; //Overflow
249
			else left_dx += dx;
263 250
			
264 251
			//Adjust velocity: update
265 252
			left_v = left_dx - left_v;
266 253
		}
267 254

  
268
		if(right_data < INVALID) {
269
			//Put new data onto data array
255
		if(right_data != INVALID && right_data != MAGNET_FAILURE) {
270 256
			right_data_array_put(right_data);
271 257

  
272 258
			//Adjust right accumulator
273 259
			dx = right_data - right_data_array_prev();
274 260

  
275
			if(right_data_array_prev()==0)
276
			  dx=0;
277

  
278
			if(dx > 5) { //underflow
279
				right_dx += dx - 1023;
280
			}
281
			else if(dx < -5) { //overflow
282
				right_dx += dx + 1023;
283
			}
284
			else {
285
				right_dx += dx;
286
			}
261
			if(right_data_array_prev()==0) dx=0;
262
			  
263
			if(dx > 512) right_dx += dx - 1023; //underflow
264
			else if(dx < -512) right_dx += dx + 1023; //overflow 
265
			else right_dx += dx;
287 266
		}
288
  
289
		
290
		}
267
	}
291 268

  
292
		//Increment timecount accumulator
293
		timecount++;
294
		
295
	//}
269
	//Increment timecount accumulator
270
	timecount++;
296 271
}
297 272

  
273
void encoders_print_data_array(void){
274
	int i;
275
	usb_puts("Left Data: Pointer is ");
276
	usb_puti(left_data_idx);
277
	usb_puts("\n\r");
278
	for(i=0; i<BUFFER_SIZE; i++){
279
		usb_puti(left_data_array[i]);
280
		usb_putc(' ');
281
	}
282
	usb_puts("\r\n");
283
	usb_puts("Right Data: Pointer is");
284
	usb_puti(right_data_idx);
285
	usb_puts("\n\r");
286
	for(i=0; i<BUFFER_SIZE; i++){
287
		usb_puti(right_data_array[i]);
288
		usb_putc(' ');
289
	}
290
	usb_puts("\r\n\r\n");
291
}
292

  
293

  
298 294
//Helper Functions
299 295
inline void left_data_array_put(unsigned short int value) {
300 296
	if(left_data_idx == BUFFER_SIZE-1)
......
347 343
	else
348 344
		return right_data_array[right_data_idx + 1];
349 345
}
346

  

Also available in: Unified diff