Index: encoders.c
===================================================================
--- encoders.c	(revision 853)
+++ encoders.c	(revision 854)
@@ -38,38 +38,23 @@
 inline unsigned int right_data_array_prev(void);
 inline unsigned int right_data_array_bottom(void);
 
-//RING_BUFFER_NEW(enc_buffer, BUFFER_SIZE, short int);
 
-void encoder_recv_complete(void);
-
 void encoder_recv_complete(){
-   encoder_buf_index = 0;
-   //  usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n");
-   //   usb_puts("\r\n");
-  data_ready++;
+  	encoder_buf_index = 0;
+	data_ready++;
   
-  //delay_ms(ENCODER_DELAY);
-
-   spi_transfer(5);
+   	spi_transfer(5);
 }
 
-void put_bin(char data){
-  int i;
-
-  for(i=7;i>=0;i--)
-    usb_puti((data>>i)&1);
-  usb_puts(" ");
-}
-
-/**
- * Initializes the encoders variables
- **/
+/** 
+* @brief Initializes encoder variables and the hardware interface.
+*/
 void encoders_init(void){
 	int i;
 	
 	data_ready=0;
 
-	spi_init(encoder_recv/*put_bin*/, encoder_recv_complete);
+	spi_init(encoder_recv, encoder_recv_complete);
 	encoder_buf_index = 0;
 	left_data_buf = 0;
 	right_data_buf= 0;
@@ -90,11 +75,10 @@
 		right_data_array[i] = 0;
 	}
 	spi_transfer(5);
-	//usb_puts("\tencoders.c Debug: ENCODERS INITIALIZED\n\r");
 }
 
 /**
- * Returns the specified encoders value
+ * @brief Returns the specified encoders value
  *
  * @param encoder this is the encoder that you want to read. Valid arguments
  *          are LEFT and RIGHT
@@ -102,70 +86,77 @@
  * @return the value of the specified encoder
  **/
 int encoder_read(char encoder){
-	if(encoder==LEFT){
-		//usb_puts("\tencoders.c Debug: PRINTING LEFT ENCODER VALUE\n\r");
-		return left_data;
-	}
-	else if(encoder==RIGHT){
-		//usb_puts("\tencoders.c Debug: PRINTING RIGHT ENCODER VALUE\n\r");
-		return right_data;
-	}
-	else{
-		//usb_puts("\tencoders.c Debug: INVALID ENCODER - USE LEFT/RIGHT\n\r");
-		 return -1;
-	}
+	
+	if(encoder==LEFT) return left_data;
+	else if(encoder==RIGHT) return right_data;
+	else return -1;
 }
 
-int encoder_change(char encoder){
-	return 0;
-}
 
+/** 
+* @brief Outputs encoder direction as FORWARD OR BACK
+* A STUB! DO NOT use.
+* 
+* @param encoder The encoder you want the direction of.
+* Valid arguments are right and left.
+* 
+* @return FORWARD or BACK (the constants)
+*/
 char encoder_direction(char encoder){
 	return 0;
 }
+
 /**
- * Gets the velocity of the specified encoder.
+ * Gets the total distance covered by the specified encoder (in encoder clicks)
  *
  * @param encoder the encoder that you want to read, use LEFT or RIGHT
  *
- * @return The velocity of the specified encoder.
+ * @return The distance covered by the specified encoder.
  **/
 int encoder_get_dx(char encoder) {
-	if(encoder==LEFT)
-		return left_dx;
-	else if(encoder==RIGHT)
-		return right_dx;
+	
+	if(encoder==LEFT) return left_dx;
+	else if(encoder==RIGHT) return right_dx;
 	else return -1;
 }
 
 /**
- * Resets the value of the velocity global variable for the specified
+ * Resets the distance accumulator for the specified
  *  encoder.
  *
- * @param encoder the encoder that you want to modify
+ * @param encoder the encoder that you want to reset distance for
  **/
 void encoder_rst_dx(char encoder) {
-	if(encoder==LEFT)
-		left_dx = 0;
-	else if(encoder==RIGHT)
-		right_dx = 0;
+	
+	if(encoder==LEFT) left_dx = 0;
+	else if(encoder==RIGHT) right_dx = 0;
 }
 
-/**
- * Returns the current time count for the encoders.
- **/
+/** 
+* @brief Returns the number of encoder reads that have occurred.
+* 
+* @return The time count.
+*/
 int encoder_get_tc(void) {
 	return timecount;
 }
 
-/**
- * Resets the time count for the encoders.
- **/
+/** 
+* @brief Resets the encoder read counter.
+*/
 void encoder_rst_tc(void) {
 	timecount = 0;
 }
 
 
+/** 
+* @brief Returns the approximated instantaneous velocity of the robot
+* in terms of encoder clicks.
+* 
+* @param encoder RIGHT or LEFT - the wheel you want the velocity for.
+* 
+* @return The instantaneous velocity for the given wheel.
+*/
 int encoder_get_v(char encoder){
 	int last, res=0;
 	
@@ -196,6 +187,13 @@
 	return res;
 }
 
+
+/** 
+* @brief Waits until n encoder reads have occurred.
+* Counter is reset on functions exit.
+* 
+* @param n 
+*/
 void encoder_wait(int n){
 	while(data_ready<n);
 	data_ready=0;
@@ -228,28 +226,38 @@
 	encoder_buf_index = (encoder_buf_index + 1) % 5;
 
 	if(encoder_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;
+		
+		/*Error handling for the left encoder*/ 
+		if(!(left_data_buf & OCF)) 
+			left_data = ENCODER_DATA_NOT_READY; 
+		if(left_data_buf & (COF | LIN))  
+			left_data = ENCODER_MISALIGNED;
+		else if((left_data_buf & MagINCn) && (left_data_buf & MagDECn)) 
+			left_data = ENCODER_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;
+		/*Error handling for the right encoder*/ 
+	 	if(!(right_data_buf & OCF))
+			right_data = ENCODER_DATA_NOT_READY;	
+		if(right_data_buf & (COF | LIN)) 
+			right_data = ENCODER_MISALIGNED;
+		else if ((right_data_buf & MagINCn)  && (right_data_buf & MagDECn)) 
+			right_data = ENCODER_MAGNET_FAILURE;
 		else right_data = (right_data_buf>>5) & 1023;
 			
   		left_data_buf = 0;
 		right_data_buf = 0;
 
-		
-		if(left_data != INVALID && left_data != MAGNET_FAILURE) {
+		/*Above 1023 is invalid data*/	
+		if(!(left_data > 1023)) {
 			left_data_array_put(left_data);
 
 			//Adjust left accumulator
-			dx = left_data_array_prev() - left_data;
+			dx = left_data - left_data_array_prev();
 			
 			//Adjust velocity: save last dx
 			left_v = left_dx;
-
+			
 			if(left_data_array_prev()==0)  dx=0;
 
 			if(dx > 512) left_dx += dx - 1023; //Underflow
@@ -260,7 +268,8 @@
 			left_v = left_dx - left_v;
 		}
 
-		if(right_data != INVALID && right_data != MAGNET_FAILURE) {
+		/*Above 1023 is invalid data*/	
+		if(!(right_data > 1023)) {
 			right_data_array_put(right_data);
 
 			//Adjust right accumulator
@@ -279,27 +288,6 @@
 }
 
 
-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)
