Colony

#include "encoders.h"

char encoder_buf_index;

unsigned int left_data, right_data;

unsigned int left_data_array[BUFFER_SIZE];

unsigned int right_data_array[BUFFER_SIZE];

int left_data_idx;

int right_data_idx;

int left_dx;

int right_dx;

long int timecount;

volatile short int data_ready;

void encoder_recv(unsigned int, unsigned int);

//Helper Function Prototypes

inline void left_data_array_put(unsigned short int value);

inline unsigned int left_data_array_top(void);

inline unsigned int left_data_array_prev(void);

inline unsigned int left_data_array_bottom(void);

inline void right_data_array_put(unsigned short int value);

inline unsigned int right_data_array_top(void);

inline unsigned int right_data_array_prev(void);

inline unsigned int right_data_array_bottom(void);

/**

 * @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) return left_data;

	else if(encoder==RIGHT) return right_data;

	else return -1;

}

/** 

* @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 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 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;

	else return -1;

}

/**

 * Resets the distance accumulator for the specified

 *  encoder.

 *

 * @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;

}

/** 

* @brief Returns the number of encoder reads that have occurred.

* 

* @return The time count.

*/

int encoder_get_tc(void) {

	return timecount;

}

/** 

* @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){

	if (encoder == LEFT)

		return left_data_array_bottom() - left_data_array_top();

	if (encoder == RIGHT)

		return right_data_array_top() - right_data_array_bottom();

	return -1; /* TODO: velocity could be -1, use another value for error */

}

/** 

* @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;

}

//Full reads occur every 40 microseconds. This function should be called

//every 8 microseconds.

void encoder_recv(unsigned int left_data_buf, unsigned int right_data_buf){

	short int dx;

	left_data = left_data_buf;

	right_data = right_data_buf;

	/*Above 1023 is invalid data*/	

	if(!(left_data > 1023)) {

		left_data_array_put(left_data);

		//Adjust left accumulator

		dx = - left_data + left_data_array_prev();

		if(left_data_array_prev()==0)  dx=0;

		if(dx > 512) left_dx += dx - 1023; //Underflow

		else if(dx < -512) left_dx += dx + 1023; //Overflow

		else left_dx += dx;

	}

	/*Above 1023 is invalid data*/	

	if(!(right_data > 1023)) {

		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 > 512) right_dx += dx - 1023; //underflow

		else if(dx < -512) right_dx += dx + 1023; //overflow 

		else right_dx += dx;

	}

	//Increment timecount accumulator

	timecount++;

}

//Helper Functions

inline void left_data_array_put(unsigned short int value) {

	if(left_data_idx == BUFFER_SIZE-1)

		left_data_idx = 0;

	else

		left_data_idx++;

	left_data_array[left_data_idx] = value;

}

inline unsigned int left_data_array_top(void) {

	return left_data_array[left_data_idx];

}

inline unsigned int left_data_array_prev(void) {

	if(left_data_idx == 0)

		return left_data_array[BUFFER_SIZE-1];

	else

		return left_data_array[left_data_idx - 1];

}

inline unsigned int left_data_array_bottom(void) {

	if(left_data_idx == BUFFER_SIZE-1)

		return left_data_array[0];

	else

		return left_data_array[left_data_idx + 1];

}

inline void right_data_array_put(unsigned short int value) {

	if(right_data_idx == BUFFER_SIZE-1)

		right_data_idx = 0;

	else

		right_data_idx++;

	right_data_array[right_data_idx] = value;

}

inline unsigned int right_data_array_top(void) {

	return right_data_array[right_data_idx];

}

inline unsigned int right_data_array_prev(void) {

	if(right_data_idx == 0)

		return right_data_array[BUFFER_SIZE-1];

	else

		return right_data_array[right_data_idx - 1];

}

inline unsigned int right_data_array_bottom(void) {

	if(right_data_idx == BUFFER_SIZE-1)

		return right_data_array[0];

	else

		return right_data_array[right_data_idx + 1];

}

/**

 * 

 * @file encoders.h

 * @brief Contains functions for reading encoder values.

 *

 * Contains high and low level functions for reading encoders

 * including reading out total distance covered, and 

 * eventually velocity.

 *	

 * @author Colony Project, CMU Robotics Club

*/

/**

 * @addtogroup encoders

 * @{

 **/

#ifndef __ENCODERS_H__

#define __ENCODERS_H__

#ifndef LEFT

	#define LEFT 0

#endif

#ifndef RIGHT

	#define RIGHT 1

#endif

/** @brief Magnet misaligned - likely distance from encoder problem. **/

#define ENCODER_MAGNET_FAILURE 1025

/** @brief Encoder misaligned - likely on XY plane. **/

#define ENCODER_MISALIGNED 1027

/** @brief Not enough time has passed - encoders not initialized in hardware. **/

#define ENCODER_DATA_NOT_READY 1026

//delay_ms argument after a full read is complete

#define ENCODER_DELAY 20

//Data invalid flags (hardware failure):

#define OCF _BV(4)

#define COF _BV(3)

//Data invalid alarm (May be invalid):

#define LIN _BV(2)

#define MagINCn _BV(1)

#define MagDECn _BV(0)

#define BUFFER_SIZE 23

/** @brief Initialize encoders. **/

void encoders_init(void);

/** @brief Read instantaneous encoder value. **/

int encoder_read(char);

/** @brief Currently a stub - DO NOT Use. **/

char encoder_direction(char encoder);

/** @brief Get total distance traveled. **/

int encoder_get_dx(char encoder);

/** @brief Reset distance counter. **/

void encoder_rst_dx(char encoder);

/** @brief Get time count: The number of encoder reads that have occurred. **/

int encoder_get_tc(void);

/** @brief Reset the time count. **/

void encoder_rst_tc(void);

/** @brief Still untested, so use at your own risk. **/

int encoder_get_v(char encoder);

/** @brief Waits for the next encoder reading, then returns. **/

void encoder_wait( int nReadings );

/**@}**/ //end group

#endif