Index: encoders.c
===================================================================
--- encoders.c	(revision 808)
+++ encoders.c	(revision 809)
@@ -22,7 +22,6 @@
 int left_v;
 int right_v;
 
-volatile int recv_count;
 
 volatile short int data_ready;
 
@@ -45,13 +44,11 @@
 
 void encoder_recv_complete(){
 
-int dx;
-
    //  usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n");
    //   usb_puts("\r\n");
   data_ready++;
   
-  delay_ms(ENCODER_DELAY);
+  //delay_ms(ENCODER_DELAY);
 
    spi_transfer(5);
 }
@@ -64,6 +61,9 @@
   usb_puts(" ");
 }
 
+/**
+ * Initializes the encoders variables
+ **/
 void encoders_init(void){
 	int i;
 	
@@ -76,7 +76,6 @@
 	left_data = -1;
 	right_data = -1;
 	
-	recv_count=0;
 	
 	left_v=0;
 	right_v=0;
@@ -93,6 +92,14 @@
 	spi_transfer(5);
 }
 
+/**
+ * Returns the specified encoders value
+ *
+ * @param encoder this is the encoder that you want to read. Valid arguments
+ *          are LEFT and RIGHT
+ *
+ * @return the value of the specified encoder
+ **/
 int encoder_read(char encoder){
 	if(encoder==LEFT)
 		return left_data;
@@ -108,7 +115,13 @@
 char encoder_direction(char encoder){
 	return 0;
 }
-
+/**
+ * Gets the velocity of the specified encoder.
+ *
+ * @param encoder the encoder that you want to read, use LEFT or RIGHT
+ *
+ * @return The velocity of the specified encoder.
+ **/
 int encoder_get_dx(char encoder) {
 	if(encoder==LEFT)
 		return left_dx;
@@ -117,6 +130,12 @@
 	else return -1;
 }
 
+/**
+ * Resets the value of the velocity global variable for the specified
+ *  encoder.
+ *
+ * @param encoder the encoder that you want to modify
+ **/
 void encoder_rst_dx(char encoder) {
 	if(encoder==LEFT)
 		left_dx = 0;
@@ -124,10 +143,16 @@
 		right_dx = 0;
 }
 
+/**
+ * Returns the current time count for the encoders.
+ **/
 int encoder_get_tc(void) {
 	return timecount;
 }
 
+/**
+ * Resets the time count for the encoders.
+ **/
 void encoder_rst_tc(void) {
 	timecount = 0;
 }
@@ -172,129 +197,100 @@
 //Full reads occur every 40 microseconds. This function should be called
 //every 8 microseconds.
 void encoder_recv(char data){
-	//usb_puti(buf_index);
 	short int dx;
 	
-	recv_count++;
-	//if(++recv_count==ENCODER_DELAY){
-			
-		recv_count=0;
+	//Parse the encoder data, comes in over 5 bytes 16 bits per encoder,
+	// second is offset by 1 bit.
+	switch(buf_index){	
+	case 0: 
+		right_data_buf |= ((short)data)<<8 & 0xff00;
+		break;
+	case 1:
+		right_data_buf |= ((short)data) & 0xff;
+		break;
+	case 2:
+		left_data_buf |= (((short)data) << 9) & (0x7F << 9);
+		break;
+	case 3:
+		left_data_buf |= (((short)data) << 1) & (0xFF<<1);
+		break;
+	case 4: left_data_buf |= (((short)data)>>7) & 0x1;
+	}	
 		
-		if(buf_index == 0)
-		  right_data_buf |= ((short)data)<<8 & 0xff00;
+	buf_index = (buf_index + 1) % 5;
 
-		else if (buf_index == 1)
-		  right_data_buf |= ((short)data) & 0xff;
-		
-		else if (buf_index == 2)
-			left_data_buf |= (((short)data) << 9) & (0x7F << 9);
-		
-		else if (buf_index == 3)
-			left_data_buf |= (((short)data) << 1) & (0xFF<<1);
-		
-		else if (buf_index == 4)
-			left_data_buf |= (((short)data)>>7) & 0x1;
-
-		
-		buf_index = (buf_index + 1) % 5;
-
-		if(buf_index==0) {
-		  if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF)){
-				left_data = INVALID;
-		  }
+	if(buf_index==0) {
+		  
+		if(left_data_buf & (COF | LIN) || !(left_data_buf & OCF))  left_data = INVALID;
+		else if(((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)) left_data = MAGNET_FAILURE;
+		else left_data = (left_data_buf>>5) & 1023;
 			
-		  else if(((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)){
-			left_data = MAGNET_FAILURE;
-		  }
-		  else{
-			left_data = (left_data_buf>>5) & 1023;
-		  }
+		if(right_data_buf & (COF | LIN) || !(right_data_buf & OCF)) right_data = INVALID;
+		else if ( ((right_data_buf & MagINCn) > 0)  && ((right_data_buf & MagDECn) > 0)) right_data = MAGNET_FAILURE;
+		else right_data = (right_data_buf>>5) & 1023;
 			
-		  if(right_data_buf & (COF | LIN) || !(right_data_buf & OCF)){
-			right_data = INVALID;
-		  }
-		  else if ( ((left_data_buf & MagINCn) > 0)  && ((left_data_buf & MagDECn) > 0)){
-			left_data = MAGNET_FAILURE;
-		  }
-		  else{
-			right_data = (right_data_buf>>5) & 1023;
-		  }
-		
+  		left_data_buf = 0;
+		right_data_buf = 0;
 
-			//if(left_data!=INVALID || right_data!=INVALID){
-			//  usb_puts("{");usb_puti(left_data);usb_puts(",");usb_puti(right_data);usb_puts("}\r\n");
-			//}
-			
-			  
-  
-  			left_data_buf = 0;
-			right_data_buf = 0;
-
 		
-		if(left_data < INVALID) {
-			//Put new data onto data array
+		if(left_data != INVALID && left_data != MAGNET_FAILURE) {
 			left_data_array_put(left_data);
 
 			//Adjust left accumulator
-			//dx = left_data - left_data_array_prev();
 			dx = left_data_array_prev() - left_data;
 			
 			//Adjust velocity: save last dx
 			left_v = left_dx;
 
-			/*left dx was negative on robot 7, could just be that
-			  the encoder is backwards, so this may need to change
-			  back
-			*/
+			if(left_data_array_prev()==0)  dx=0;
 
-
-			if(left_data_array_prev()==0)
-			  dx=0;
-
-			if(dx > 5) { //underflow
-				left_dx += dx - 1023;
-			}
-			else if(dx < -5) { //overflow
-				left_dx += dx + 1023;
-			}
-			else {
-				left_dx += dx;
-			}
+			if(dx > 512) left_dx += dx - 1023; //Underflow
+			else if(dx < -512) left_dx += dx + 1023; //Overflow
+			else left_dx += dx;
 			
 			//Adjust velocity: update
 			left_v = left_dx - left_v;
 		}
 
-		if(right_data < INVALID) {
-			//Put new data onto data array
+		if(right_data != INVALID && right_data != MAGNET_FAILURE) {
 			right_data_array_put(right_data);
 
 			//Adjust right accumulator
 			dx = right_data - right_data_array_prev();
 
-			if(right_data_array_prev()==0)
-			  dx=0;
-
-			if(dx > 5) { //underflow
-				right_dx += dx - 1023;
-			}
-			else if(dx < -5) { //overflow
-				right_dx += dx + 1023;
-			}
-			else {
-				right_dx += dx;
-			}
+			if(right_data_array_prev()==0) dx=0;
+			  
+			if(dx > 512) right_dx += dx - 1023; //underflow
+			else if(dx < -512) right_dx += dx + 1023; //overflow 
+			else right_dx += dx;
 		}
-  
-		
-		}
+	}
 
-		//Increment timecount accumulator
-		timecount++;
-		
-	//}
+	//Increment timecount accumulator
+	timecount++;
 }
 
+void encoders_print_data_array(void){
+	int i;
+	usb_puts("Left Data: Pointer is ");
+	usb_puti(left_data_idx);
+	usb_puts("\n\r");
+	for(i=0; i<BUFFER_SIZE; i++){
+		usb_puti(left_data_array[i]);
+		usb_putc(' ');
+	}
+	usb_puts("\r\n");
+	usb_puts("Right Data: Pointer is");
+	usb_puti(right_data_idx);
+	usb_puts("\n\r");
+	for(i=0; i<BUFFER_SIZE; i++){
+		usb_puti(right_data_array[i]);
+		usb_putc(' ');
+	}
+	usb_puts("\r\n\r\n");
+}
+
+
 //Helper Functions
 inline void left_data_array_put(unsigned short int value) {
 	if(left_data_idx == BUFFER_SIZE-1)
@@ -347,3 +343,4 @@
 	else
 		return right_data_array[right_data_idx + 1];
 }
+
