Index: encoders.c
===================================================================
--- encoders.c	(revision 770)
+++ encoders.c	(revision 771)
@@ -19,6 +19,13 @@
 int right_dx;
 long int timecount;
 
+int left_v;
+int right_v;
+
+volatile int recv_count;
+
+volatile short int data_ready;
+
 void encoder_recv(char data);
 
 //Helper Function Prototypes
@@ -37,9 +44,16 @@
 void encoder_recv_complete(void);
 
 void encoder_recv_complete(){
-  //    usb_puts("[");usb_puti(left_dx);usb_puts(",");usb_puti(right_dx);usb_puts("]\r\n");
-  //    usb_puts("\r\n");
-	spi_transfer(5);
+
+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);
+
+   spi_transfer(5);
 }
 
 void put_bin(char data){
@@ -52,6 +66,8 @@
 
 void encoders_init(void){
 	int i;
+	
+	data_ready=0;
 
 	spi_init(encoder_recv/*put_bin*/, encoder_recv_complete);
 	buf_index = 0;
@@ -59,6 +75,12 @@
 	right_data_buf= 0;
 	left_data = -1;
 	right_data = -1;
+	
+	recv_count=0;
+	
+	left_v=0;
+	right_v=0;
+	
 	//RING_BUFFER_INIT(enc_buffer,BUFFER_SIZE);
 	left_data_idx = 0;
 	right_data_idx = 0;
@@ -111,6 +133,42 @@
 }
 
 
+int encoder_get_v(char encoder){
+	int last, res=0;
+	
+	cli();
+	
+	if(encoder==LEFT){
+		if(left_data_idx==0)
+			last = BUFFER_SIZE - 1;
+		else
+			last = left_data_idx - 1;
+		res = ((int)left_data_array[last]) - ((int)left_data_array[left_data_idx]);
+	}
+	if(encoder==RIGHT){
+		if(right_data_idx==0)
+			last = BUFFER_SIZE - 1;
+		else
+			last = right_data_idx - 1;
+		res = ((int)right_data_array[right_data_idx]) - ((int)right_data_array[last]);
+	}
+	
+	sei();
+
+	while(res<MIN_V)//underflow
+		res+=1024;
+	while(res>MAX_V)//overflow
+		res-=1024;
+
+	return res;
+}
+
+void encoder_wait(int n){
+	while(data_ready<n);
+	data_ready=0;
+}
+
+
 //Full reads occur every 40 microseconds. This function should be called
 //every 8 microseconds.
 void encoder_recv(char data){
@@ -117,108 +175,124 @@
 	//usb_puti(buf_index);
 	short int dx;
 	
-	if(buf_index == 0)
-	  right_data_buf |= ((short)data)<<8 & 0xff00;
+	recv_count++;
+	//if(++recv_count==ENCODER_DELAY){
+			
+		recv_count=0;
+		
+		if(buf_index == 0)
+		  right_data_buf |= ((short)data)<<8 & 0xff00;
 
-	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;
-	  }
+		else if (buf_index == 1)
+		  right_data_buf |= ((short)data) & 0xff;
 		
-	  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 (buf_index == 2)
+			left_data_buf |= (((short)data) << 9) & (0x7F << 9);
 		
-	  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;
-	  }
-	
+		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;
 
-		//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;
+		buf_index = (buf_index + 1) % 5;
 
-	
-	if(left_data < INVALID) {
-		//Put new data onto data array
-		left_data_array_put(left_data);
+		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;
+		  }
+			
+		  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;
+		  }
+		
 
-		//Adjust left accumulator
-		//dx = left_data - left_data_array_prev();
-		dx = left_data_array_prev() - left_data;
+			//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;
 
-		/*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 < INVALID) {
+			//Put new data onto data array
+			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(dx > 5) { //underflow
-			left_dx += dx - 1023;
+			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;
+			}
+			
+			//Adjust velocity: update
+			left_v = left_dx - left_v;
 		}
-		else if(dx < -5) { //overflow
-			left_dx += dx + 1023;
-		}
-		else {
-			left_dx += dx;
-		}
-	}
 
-	if(right_data < INVALID) {
-		//Put new data onto data array
-		right_data_array_put(right_data);
+		if(right_data < INVALID) {
+			//Put new data onto data array
+			right_data_array_put(right_data);
 
-		//Adjust right accumulator
-		dx = right_data - right_data_array_prev();
+			//Adjust right accumulator
+			dx = right_data - right_data_array_prev();
 
-		if(right_data_array_prev()==0)
-		  dx=0;
+			if(right_data_array_prev()==0)
+			  dx=0;
 
-		if(dx > 5) { //underflow
-			right_dx += dx - 1023;
+			if(dx > 5) { //underflow
+				right_dx += dx - 1023;
+			}
+			else if(dx < -5) { //overflow
+				right_dx += dx + 1023;
+			}
+			else {
+				right_dx += dx;
+			}
 		}
-		else if(dx < -5) { //overflow
-			right_dx += dx + 1023;
+  
+		
 		}
-		else {
-			right_dx += dx;
-		}
-	}
-	}
 
-	//Increment timecount accumulator
-	timecount++;
+		//Increment timecount accumulator
+		timecount++;
+		
+	//}
 }
 
 //Helper Functions
