/branches/encoders/code/projects/libdragonfly/encoders.c - Diff - Colony - Robotics Club

Revision 854

Added by Justin Scheiner over 15 years ago

Did a lot of cleaning up and commenting. Made the error codes returned
by encoder_read more explicit - so if you get weird values check the documentation.

A lot of stubs and incomplete code, I made these things explicit in the source and documentation.

     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;
-...
     		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
-...
      * @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;
-...
     	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;
-...
     	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
-...
     			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
-...
+    }
     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)

Also available in: Unified diff

Project

General

Profile

Colony

Revision 854