Colony

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file rangefinder.c

 * @brief Rangefinders

 *

 * Implementation of functions for rangefinder use.

 *

 * @author Colony Project, CMU Robotics Club

 **/

/*

  Authors: James Kong and Greg Tress

  Last Modified: 4/30/06 by James

  -Started log_distance conversion function !!!NOT COMPLETE!!!

  -Cleaning up comments

  -----------------

  rangefinder.c

  Using Sharp GP2D02 IR Rangefinder

  Vin is the input to the rangefinder, designated RANGE_CTRL.

  Vout is the output from the rangefinder, designated RANGE_IN# where # is the rangefinder you are reading from

  Expected Initial Conditions:

  Vin is high and Vout should read high.

  Usage:

  1.) Set Vin low. Vout should read low.

  2.) Wait for high on Vout.

  3.) Begin clocking Vin and reading 8 bits from Vout (MSB first).

  4.) Set Vin high for 2ms or more to turn off rangefinder

*/

#include <avr/pgmspace.h>

#include "rangefinder.h"

#include "analog.h"

#include "dio.h"

/*

  read_distance returns the 8-bit reading from the rangefinder

  parameters:

  range_id - dio pin set as the rangefinder Vout [i.e. RANGE_IN0]

  NOTE:

  The Sharp GD2D02 returns values on a decreasing logrithmic scale.

  So higher values correspond to closer distances.  Use linearize_distance to convert to normal centimeter scale.

  Also, when reading distances closer than 8cm, the Sharp GD2D02 will return lower values than the values at 8cm.

  At this point, we are only reading from one rangefinder [RANGE_IN0].

*/

// constants

/* Nasty IR approximation table

   I'm using this for the heck of it.  We can do whatever.

   Note the minimum value is .4V (20), and the maximum is 2.6V (133).

   Gives distance in mm.

   excel formula(valid for inputs 20-133):  ROUND(2353.6*(E2^(-1.1146))*10,0)

   This is only valid for the GP2D12, with objects directly ahead and more than

   10cm from the detector.  See the datasheet for more information.

*/

static int IR_dist_conversion[72] PROGMEM = {

	327,315,303,291,281,271,262,253,245,238,231,224,218,212,206,200,

	195,190,185,181,177,173,168,165,161,158,155,151,148,145,143,140,

	137,134,132,130,127,125,123,121,119,117,115,114,111,110,108,106,

	105,104,102,100,99,98,97,95,94,93,91,90,89,88,87,86,84,83,83,82,

	81,80,79,78

};

/**

 * @defgroup rangefinder Rangefinder

 * @brief Functions for using the IR rangefinders

 * 

 * Functions for using the IR rangefinders.

 *

 * @{

 **/

/**

 * Initializes the rangefinders. This must be called before

 * range_read_distance.

 *

 * @see range_read_distance

 **/

void range_init(void)

{

  digital_output(_PIN_B4,0);

}

/**

 * Reads the distance measured by one of the rangefinders.

 * This distance is in arbitrary units.

 *

 * @param range_id the rangefinder to use. This should be one

 * of the constants IR1 - IR5.

 *

 * @return the distance measured by the rangefinder

 *

 * @see range_init

 **/

int range_read_distance(int range_id) {

  return linearize_distance(analog8(range_id));

}

/**

 * Transforms distance readings from logarithmic to linear scale.

 * This probably isn't the function you are looking for.

 *

 * Note: pgm_read_word() needs additional testing

 *

 * @param value the 8-bit analog value from rangefinder

 *

 * @return linearized distance reading from rangefinder (integer in [101,800])

 **/

int linearize_distance(int value) {

  if(value < MIN_IR_ADC8) {

    return -1;

  } else if(value > MAX_IR_ADC8) {

    return -1;

  } else {

    return pgm_read_word(&(IR_dist_conversion[value - MIN_IR_ADC8]));

  }

}

/** @} **/ //end defgroup

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file rangefinder.h

 * @brief Contains rangefinder declarations and functions

 * 

 * Contains functions and definitions for the use of

 * IR rangefinders.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#ifndef _RANGEFINDER_H_

#define _RANGEFINDER_H_

/**

 * @addtogroup rangefinder

 * @{

 **/

/** @brief IR Rangefinder 1 **/

#define IR1 6

/** @brief IR Rangefinder 2 **/

#define IR2 5

/** @brief IR Rangefinder 3 **/

#define IR3 4

/** @brief IR Rangefinder 4 **/

#define IR4 3

/** @brief IR Rangefinder 5 **/

#define IR5 2

/** @brief smallest meaningful rangefinder reading (logarithmic scale) **/

#define MIN_IR_ADC8 27

/** @brief largest meaningful rangefinder reading (logarithmic scale) **/

#define MAX_IR_ADC8 98

/** @brief Initialize the rangefinders **/

void range_init(void);

/** @brief Read the distance from a rangefinder **/

int range_read_distance(int range_id);

/** @brief Convert logarithmic-scale distance readings to a linear scale **/

int linearize_distance(int value);

/** @} **/ //end addtogroup

#endif

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file bom.c

 * @brief Implementation for using the BOM

 *

 * Contains functions for using the Bearing and Orientation Module (BOM)

 *

 * @author Colony Project, CMU Robotics Club

 **/

#include "bom.h"

#include "dio.h"

#include "serial.h"

#include "analog.h"

//On the original BOM1.0, the emmitter angular order does not match the analog mux order

//so you need to iterate through the mux index in the following order if you want to get

//the detector readings in order:

static const char lookup[16] = {7,6,5,0xe,1,4,3,2,0xf,0,0xd,8,0xc,0xb,9,0xa};

/* *****************************

 * BOM Vector Component Tables *

 **************************** **/

/*

 * The x component of each BOM detector (indexed from 0 to 15)

 * was calculated using the following formula:

 *

 *		x_comp[i] = fix(25 * cos ( 2 * pi / 16 * i) )

 *

 * where "fix" rounds towards 0. If the BOM detectors were superimposed

 * onto a 2 dimensional Cartesian space, this effectively calculates the

 * x component of the emitter vector where emitter 0 corresponds to an

 * angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.

 */

static const signed int x_comp[16] = {

	25,

	23,

	17,

	9,

	0,

	-9,

	-17,

	-23,

	-25,

	-23,

	-17,

	-9,

	0,

	9,

	17,

	23

};

/*

 * The y component of each BOM detector (indexed from 0 to 15)

 * was calculated using the following formula:

 *

 *		y_comp[i] = fix(25 * sin ( 2 * pi / 16 * i) )

 *

 * where "fix" rounds towards 0. If the BOM detectors were superimposed

 * onto a 2 dimensional Cartesian space, this effectively calculates the

 * x component of the emitter vector where emitter 0 corresponds to an

 * angle of 0 radians, 4 -> pi/2, 8 -> pi, ect.

 */

static signed int y_comp[16] = {

	0,

	9,

	17,

	23,

	25,

	23,

	17,

	9,

	0,

	-9,

	-17,

	-23,

	-25,

	-23,

	-17,

	-9

};

// internal function prototypes

static void bom_select(char which);

/*

 Bk R Y (Analog)

---------

 Green

 Blue

 White

---------

 Blue

 White

*/

/*

the analog pin definitions from dio.h DO NOT work here,

so we must use PF0 from avrgcc (as opposed to _PIN_F0).

BUT the dio pin definitions from dio.h must be used (no PE...).

also, _PIN_E2 is initialized to high for some reason,

which turns the BOM on when the robot is turned on.

WORK-AROUND: call digital_output(_PIN_E2,0) at some point.

*/

#define MONKI PF0         //analog (yellow)

//------------------------//

#define MONKL _PIN_E2     //green

#define MONK1 _PIN_E3     //blue

#define MONK0 _PIN_E4     //white

//------------------------//

#define MONK3 _PIN_E6     //blue

#define MONK2 _PIN_E7     //white

#define BOM_VALUE_THRESHOLD 150 //200

#define NUM_BOM_LEDS 16

/*

  *The following pin definitions are for the BOM v1.5

  */

#define BOM_MODE	_PIN_E2	//dio0

#define BOM_STROBE	_PIN_E3	//dio1

#define BOM_DATA	_PIN_A0 //servo0

#define BOM_CLOCK	_PIN_A1	//servo1

#define BOM_S0		_PIN_E5	//dio3

#define BOM_S1		_PIN_E4	//dio2

#define BOM_S2		_PIN_E7	//dio4

#define BOM_S3		_PIN_E6	//dio5

#define BOM_OUT		PF0		//analog(yellow)

/**

 * @defgroup bom BOM (Bearing and Orientation Module)

 * @brief Functions for dealing with the BOM.

 *

 * The Bearing and Orientation Module / Barrel of Monkeys / BOM

 * is a custom sensor designed and built by the Colony Project.

 * It consists of a ring of 16 IR emitters and 16 IR detectors.

 * The BOM is most often use to determine the direction of other

 * robots. This module contains functions for controlling the BOM.

 *

 * Include bom.h to access these functions.

 *

 * @{

 **/

static unsigned int bom_val[NUM_BOM_LEDS];

static volatile char bom_type = BOM10;

static int select_pins[4];

static int analog_pin;

/**

 * Initializes the BOM.

 * Call bom_init before reading bom values or turning bom leds.

 *

 * @bugs INCOMPLETE - No utilization of BOM1.5 RSSI capability. Probably leave this out

 * until Cornell and Pras return

 * 

 * @see bom_refresh, bom_leds_on, bom_leds_off

 **/

void bom_init(char type) {

    bom_type = type;

    switch(bom_type) {

    case BOM10:

		select_pins[0] = MONK0; 

		select_pins[1] = MONK1;

		select_pins[2] = MONK2;

		select_pins[3] = MONK3;

		analog_pin = MONKI;

        break;

    case BOM15:

        //Sets BOM1.5 to normal [BOM] mode

        digital_output(BOM_MODE, 0);

		select_pins[0] = BOM_S0; 

		select_pins[1] = BOM_S1;

		select_pins[2] = BOM_S2;

		select_pins[3] = BOM_S3;

		bom_set_leds(BOM_ALL);

		analog_pin = BOM_OUT;

        break;

    case RBOM:

        break;

    //default:

    }

}

/**

 * Iterates through each bit in the bit_field. For each set bit, sets the corresponding bom select bits

 *    and updates the corresponding bom value with an analog_get8 reading.  analog_init and bom_init

 *    must be called for this to work. Must call this before reading BOM values!

 *

 *

 * @param bit_field specifies which elements in bom_val[] should be updated. Use BOM_ALL to refresh all values.

 *    Ex. if 0x0003 is passed, bom_val[0] and bom_val[1] will be updated.

 *

 * @see bom_get

 **/

void bom_refresh(int bit_field) {

    int i;

	int loop_was_running = 0;

	//Check analog loop status

    if(analog_loop_status() == ADC_LOOP_RUNNING) {

		loop_was_running = 1;

		analog_stop_loop();

	}

	//Read BOM values

    for(i = 0; i < NUM_BOM_LEDS; i++) {

        if(bit_field & 0x1) {

            bom_select(i);

            bom_val[i] = analog_get8(analog_pin);

        }

        bit_field = bit_field >> 1;

    }

	//Restore analog loop status

	if(loop_was_running)

		analog_start_loop();

}

/**

 * Gets the bom reading from bom_val[which].  Call bom_refresh beforehand to read new bom values.

 *

 * @pre must call bom refresh first

 *

 * @param which which bom value to return

 *

 * @return the bom value

 *

 * see bom_refresh

 **/

int bom_get(int which) {

    return bom_val[which];

}

/** 

 * Compares all the values in bom_val[] and returns the index to the lowest (max) value element.

 *

 * @pre must call bom refresh

 * @return index to the lowest (max) bom value element.  -1 if no value is lower than

 *    BOM_VALUE_THRESHOLD

 **/

int bom_get_max(void) {

    int i, lowest_val, lowest_i;

    lowest_i = -1;

    lowest_val = 255;

    for(i = 0; i < NUM_BOM_LEDS; i++) {

        if(bom_val[i] < lowest_val) {

            lowest_val = bom_val[i];

            lowest_i = i;

        }

    }

    if(lowest_val < BOM_VALUE_THRESHOLD)

        return lowest_i;

    else

        return -1;

}

/** 

 * Compute the net resultant BOM IR vector by scaling each IR unit vector by its intensity

 *		and summing over all IR LEDs.

 *

 * @param v  Pointer to Vector struct to be filled by this function with an x and y net vector

 *		component.

 *

 * @param usrBOMvals  Pointer to array which holds 16 raw BOM readings which can be used if user

 *		has already collected BOM information instead of collecting a new data set from the BOM.

 *

 * @return  Exit status - Zero for success; negative on error.

 **/

int bom_get_vector(Vector* v, int* usrBOMvals) {

	/* Store current BOM readings and use them as a weighting factor */

	int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

	/* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")

	 * components. Calculated by multiplying the intensity by the x and y

	 * component respectively (x and y components are stored in the tables

	 * above). */

	int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

	int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

	/* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")

	 * components for the entire robot. */

	long net_x_comp = 0;

	long net_y_comp = 0;

	int i = 0;

	/* BOM intensity is actually measured as more intense = closer to 0 */

	if (usrBOMvals) {

		/* Use BOM values collected by user */

		for (i = 0; i < 16; i++) {

			intensity[i] = 255 - usrBOMvals[i];

		}

	} else {

		/* Collect new set of BOM data */

		bom_refresh(BOM_ALL);

		for (i = 0; i < 16; i++) {

			intensity[i] = 255 - bom_get(i);

		}

	}

	/* Calculate weighted vector components and accumulate vector sum */

	for (i = 0; i < 16; i++) {

		weighted_x_comp[i] = intensity[i] * x_comp[i];

		weighted_y_comp[i] = intensity[i] * y_comp[i];

		net_x_comp += weighted_x_comp[i];

		net_y_comp += weighted_y_comp[i];

	}

	/* Fill the Vector struct */

	v->x = net_x_comp;

	v->y = net_y_comp;

	return 0;

}

/** 

 * Compute the normalized net resultant BOM IR vector by scaling each IR unit vector by its

 *		intensity and summing over all IR LEDs.

 *

 * @param v  Pointer to Vector struct to be filled by this function with an x and y net vector

 *		component.

 *

 * @param usrBOMvals  Pointer to array which holds 16 raw BOM readings which can be used if user

 *		has already collected BOM information instead of collecting a new data set from the BOM.

 *

 * @return  Exit status - Zero for success; negative on error.

 **/

int bom_get_norm_vector(Vector* v, int* usrBOMvals) {

	/* Store current BOM readings and use them as a weighting factor */

	int intensity[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

	/* Arrays for storing the weighted x ("Rightness") and y ("Forwardness")

	 * components. Calculated by multiplying the intensity by the x and y

	 * component respectively (x and y components are stored in the tables

	 * above). */

	int weighted_x_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

	int weighted_y_comp[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

	/* Accumulators to sum up the net x ("Rightness") and y ("Forwardness")

	 * components for the entire robot. */

	long net_x_comp = 0;

	long net_y_comp = 0;

	/* Variables used to normalize the net component values */

	int total_intensity = 0;

	int normalized_net_x_comp = 0;

	int normalized_net_y_comp = 0;

	int i = 0;

	/* BOM intensity is actually measured as more intense = closer to 0 */

	if (usrBOMvals) {

		/* Use BOM values collected by user */

		for (i = 0; i < 16; i++) {

			intensity[i] = 255 - usrBOMvals[i];

		}

	} else {

		/* Collect new set of BOM data */

		bom_refresh(BOM_ALL);

		for (i = 0; i < 16; i++) {

			intensity[i] = 255 - bom_get(i);

		}

	}

	/* Calculate weighted vector components and accumulate vector sum */

	for (i = 0; i < 16; i++) {

		weighted_x_comp[i] = intensity[i] * x_comp[i];

		weighted_y_comp[i] = intensity[i] * y_comp[i];

		net_x_comp += weighted_x_comp[i];

		net_y_comp += weighted_y_comp[i];

		total_intensity += intensity[i];

	}

	/* Normalize the resultant vector components by the total intensity */

	if (total_intensity > 0) {

		normalized_net_x_comp = net_x_comp / total_intensity;

		normalized_net_y_comp = net_y_comp / total_intensity;

	}

	/* Fill the Vector struct */

	v->x = normalized_net_x_comp;

	v->y = normalized_net_y_comp;

	return 0;

}

/** 

 * Print a histogram which shows the current BOM intensity values for each of the 16 BOM IR

 *		sensors. The function will attempt to send the histogram data over USB.

 *

 * @param curBOMvals  Pointer to an array of the current BOM values (the array must have

 *		length 16). Use this to print values you have already collected. Otherwise pass in NULL

 *		and bom_refresh() will be called and the current BOM intensity values will be collected.

 * @return  Exit status - Zero for success; negative on error.

 **/

int bom_print_usb(int* usrBOMvals) {

	int i, j, max = -1;

	int curVals[16];

	int* prtValPtr;

	if (usrBOMvals) {

		/* Use BOM values collected by user */

		prtValPtr = usrBOMvals;

		/* Find max BOM value from users values */

		for (i = 0; i < 16; i++) {

			if (max < prtValPtr[i])

				max = prtValPtr[i];

		}

	} else {

		/* Refresh and make sure the table is updated */

		bom_refresh(BOM_ALL);

		/* Record values into an array */

		for (i = 0; i < 16; i++) {

			curVals[i] = bom_get(i);

			if (max < curVals[i])

				max = curVals[i];

		}

		/* Use the current set of collected values */

		prtValPtr = curVals;

	}

	/* Display results */

	for (i = 0; i < 16; i++) {

		usb_puti(i);

		usb_puts(": ");

		usb_puti(prtValPtr[i]);

		usb_putc('\t');

		for (j = 0; j < (int)((max - prtValPtr[i]) / 5); j++) {

			usb_putc('#');

		}

		usb_puts("\r\n");

	}

	usb_puts("\r\n");

	return 0;

}

/** 

 * Computes the weighted average of all the bom readings to estimate the position (and distance) of another robot.

 *

 * @pre must call bom refresh

 * @param dist  pointer to int in which to return the estimated distance to the other robot

 * @return estimated position of the max bom value element as a fixed point value analogous to 10 times the

 *        index of the max bom value.  -1 if no value is lower than BOM_VALUE_THRESHOLD.

 **/

int bom_get_max10(int *dist) {

    int i, max;

    long long mean = 0, sum = 0;

    max = bom_get_max();

    if (max < 0)

    {

        if (dist)

        {

            *dist = -1;

        }

        return -1;

    }

    /* Record values into an array */

    for (i = 0; i < NUM_BOM_LEDS; i++) {

        int idx = ((i + (NUM_BOM_LEDS/2 - max) + NUM_BOM_LEDS) % NUM_BOM_LEDS) - (NUM_BOM_LEDS/2 - max);

        int val = 255 - bom_val[i];

        mean += idx * val;

        sum += val;

    }

    mean = (mean * 10) / sum;

    mean = (mean + NUM_BOM_LEDS*10) % (NUM_BOM_LEDS*10);

    if (dist)

    {

        *dist = 50 - sum/48;

    }

    return mean;

}

/**

 * Iterates through each bit in the bit_field. If the bit is set, the corresponding emitter will

 *    be enabled to turn on when bom_on() is called.

 *    bom_init must be called for this to work. Does nothing if a BOM1.0 is installed

 *

 * @param bit_field specifies which leds should be turned on when bom_on is called.  Use BOM_ALL to turn on all bom leds.

 *    Ex. if 0x0005 is passed, leds 0 and 2 will be turned on.

 **/

void bom_set_leds(int bit_field) {

    int i;

	unsigned int mask = 1<<(NUM_BOM_LEDS-1);

	switch(bom_type) {

    case BOM10:

        //TODO: put an assert here to alert the user that this should not be called

        break;

    case BOM15:

	    for(i=NUM_BOM_LEDS; i>0; i--)

	    {

		    //set the current bit, sending MSB first

		    digital_output(BOM_DATA, bit_field&mask);

		    //then pulse the clock

		    digital_output(BOM_CLOCK, 1);

		    digital_output(BOM_CLOCK, 0);

			mask = mask>>1;

	    }

        break;

    case RBOM:

        //add rbom code here

        break;

    }

}

/**

 * (DEPRECATED) Returns the direction of the maximum BOM reading,

 * as an integer in the range 0-15. 0 indicates to the

 * robot's right, while the rest of the sensors are

 * numbered counterclockwise. This is useful for determining

 * the direction of a robot flashing its BOM, of only one

 * robot is currently doing so. analog_init must be called

 * before this function can be used.

 *

 * @return the direction of the maximum BOM reading

 *

 * @see analog_init

 **/

int get_max_bom(void) {

    bom_refresh(BOM_ALL);

    return bom_get_max();

}

/**

 * Flashes the BOM.  If using a BOM1.5, only the emitters that have been enabled using

 * bom_set_leds will turn on.

 * 

 * @see bom_off, bom_set_leds

 **/

void bom_on(void)

{

  switch(bom_type) {

  case BOM10:

	digital_output(MONKL, 1);

	break;

  case BOM15:

	digital_output(BOM_STROBE, 1);

	break;

  case RBOM:

	break;

  }

}

/**

 * Turns off all bom leds.

 * 

 * @see bom_on

 **/

void bom_off(void)

{

  switch(bom_type) {

  case BOM10:

	digital_output(MONKL, 0);

	break;

  case BOM15:

	digital_output(BOM_STROBE, 0);

	break;

  case RBOM:

	break;

  }

}

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

//select a detector to read

static void bom_select(char which) {

	if(bom_type == BOM10)

	  which = lookup[(int)which];

    if (which&8)

      digital_output(select_pins[3], 1);

    else

      digital_output(select_pins[3], 0);

    if (which&4)

      digital_output(select_pins[2], 1);

    else

      digital_output(select_pins[2], 0);

    if (which&2)

      digital_output(select_pins[1], 1);

    else

      digital_output(select_pins[1], 0);

    if (which&1)

      digital_output(select_pins[0], 1);

    else

      digital_output(select_pins[0], 0);

}

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file bom.h

 * @brief Definitions for using the BOM

 * 

 * This file contains definitions for using the Bearing and 

 * Orientation Module (BOM).

 *

 * @author Colony Project, CMU Robotics Club

 *

 **/

#ifndef _BOM_H

#define _BOM_H

/**

 * @addtogroup bom

 * @{

 **/

/** @brief Include all elements in the 16-bit bitfield **/

#define BOM_ALL 0xFFFF

/** @brief Original BOM - No Range, No Individual LED control **/

#define BOM10     0

/** @brief BOM 1.5 - No Range, Individual LED control **/

#define BOM15   1

/** @brief RBOM - Range, Individual LED control **/

#define RBOM    2

/** @brief Struct for storing vector data. Used by bom_get_vector() functions **/

typedef struct vector {

	int x;

	int y;

} Vector;

/** @brief Initialize the bom according to bom type **/

void bom_init(char type);

/** @brief Refresh bom_val[] with new values from analog8.  analog_init and bom_init must be called for this to work. **/

void bom_refresh(int bit_field);

/** @brief Gets the bom reading from bom_val[which].  Call bom_refresh beforehand to read new bom values. **/

int bom_get(int which);

/** @brief Compares all the values in bom_val[] and returns the index to the highest value element. **/

int bom_get_max(void);

/** @brief Computes the net resultant BOM IR vector. **/

int bom_get_vector(Vector*, int*);

/** @brief Computes the normalized net resultant BOM IR vector. **/

int bom_get_norm_vector(Vector*, int*);

/** @brief Print snapshot of BOM intensity histogram over USB connection **/

int bom_print_usb(int*);

/** @brief Computes the weighted average of all the bom readings to estimate the position and distance of another robot. **/

int bom_get_max10(int *dist);

/** @brief Enables the selected bom leds on a BOM1.5 **/

void bom_set_leds(int bit_field);

/** @brief (DEPRECATED) Gets and compares all bom values.  Returns the index to the highest value element since last refresh. **/

int get_max_bom(void);

/** @brief Turns on all BOM leds, or turns on enabled leds on a BOM1.5. **/

void bom_on(void);

/** @brief Turns off all bom leds. **/

void bom_off(void);

/** @} **/

#endif

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file motor.c

 * @brief Motors

 *

 * Implementation of functions for controlling the motors.

 *

 * @author Colony Project, CMU Robotics Club

 * Much of this is taken from FWR's library, author: Tom Lauwers

 **/

#include "motor.h"

/**

 * @defgroup motors Motors

 * @brief Functions for controlling the motors.

 * Functions for controlling the motors. Found in motor.h.

 * @{

 **/

/**

 * Initializes both motors so that they can be used with future

 * calls to motor1_set and motor2_set.

 *

 * @see motors_off, motor1_set, motor2_set

 **/

void motors_init( void ) {

  // Configure counter such that we use phase correct

  // PWM with 8-bit resolution

  PORTA &= 0x0F;

  DDRA |= 0xF0;

  DDRB |= 0x60;

  //timer 1A and 1B

  TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);

  TCCR1B = _BV(WGM12) | _BV(CS10);

  //	TCCR1A = 0xA1;

  //	TCCR1B = 0x04;

  OCR1AH=0;

  OCR1AL=0;

  OCR1BH=0;

  OCR1BL=0;

}

/**

 * Sets the speed and direction of the left motor.

 * motors_init must be called before this function can be used.

 *

 * @param direction Either FORWARD or BACKWARD to set the direction of rotation.

 * @param speed The speed the motor will run at, in the range 0-255.

 * 

 * @see motor_r_set, motors_init

 **/

void motor_l_set(int direction, int speed) {

  if(direction == 0) {

    // turn off PWM first if switching directions

    if((PORTA & 0x30) != 0x10) {

      OCR1A = 0;

    }	

    PORTA = (PORTA & 0xCF) | 0x10;

    //		PORTD |= 0x10;

    //		PORTD &= 0xBF;

  } else {

    // turn off PWM first if switching directions

    if((PORTA & 0x30) != 0x20) {

      OCR1A = 0;

    }

    PORTA = (PORTA & 0xCF) | 0x20;

    //		PORTD |= 0x40;

    //		PORTD &= 0xEF;

  }

  // Set the timer to count up to speed, an 8-bit value

  OCR1AL = speed;

}

/**

 * Sets the speed and direction of the right motor.

 * motors_init must be called before this function can be used.

 *

 * @param direction Either FORWARD or BACKWARD to set the direction of rotation.

 * @param speed The speed the motor will run at, in the range 0-255.

 *

 * @see motor_l_set, motors_init

 **/

void motor_r_set(int direction, int speed) {

  if(direction == 0) {

    //		PORTD |= 0x20;

    //		PORTD &= 0x7F;

    // turn off PWM first if switching directions

    if((PORTA & 0xC0) != 0x80) {

      OCR1B = 0;

    }		

    PORTA = (PORTA & 0x3F) | 0x80;

  } else {

    //		PORTD |= 0x80;

    //		PORTD &= 0xDF;

    // turn off PWM first if switching directions

    if((PORTA & 0xC0) != 0x40) {

      OCR1B = 0;

    }		

    PORTA = (PORTA & 0x3F) | 0x40;

  }

  OCR1BL = speed;

}

/**

 * Sets the speed and direction of motor1.

 * motors_init must be called before this function can be used.

 *

 * @param direction Either FORWARD or BACKWARD to set the direction of rotation.

 * @param speed The speed the motor will run at, in the range 0-255.

 *

 * @see motor2_set, motors_init

 **/

void motor1_set(int direction, int speed) {

	motor_l_set(direction, speed);

}

/**

 * Sets the speed and direction of motor2.

 * motors_init must be called before this function can be used.

 *

 * @param direction Either FORWARD or BACKWARD to set the direction of rotation.

 * @param speed The speed the motor will run at, in the range 0-255.

 *

 * @see motor2_set, motors_init

 **/

void motor2_set(int direction, int speed) {

	motor_r_set(direction, speed);

}

/**

 * Turns off both motors.

 *

 * @see motors_init

 **/

void motors_off( void ) {

  OCR1AL = 0x0;

  OCR1BL = 0x0;

}

/**@}**///end defgroup

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file motor.h

 * @brief Contains definitions for controlling the motors

 *

 * Contains definitions and functions for controlling

 * the motors.

 *

 * @author Colony Project, CMU Robotics Club

 * Based on Tom Lauwer's Firefly Library

 **/

#ifndef _MOTOR_H

#define _MOTOR_H

#include <avr/io.h>

/**

 * @addtogroup motors

 * @{

 **/

/** @brief make the motors go forwards **/

#define FORWARD 1

/** @brief make the motors go backwards **/

#define BACKWARD 0

/** @brief Initialize the motors **/

void motors_init(void);

/** @brief Set speed and direction of motor1 

 *  @deprecated use the left motor function instead. it's more intuitive and easier to read.**/

void motor1_set(int direction, int speed);

/** @brief Set speed and direction of motor2 

 *  @deprecated use the right motor function instead. it's more intuitive and easier to read.**/

void motor2_set(int direction, int speed);

/** @brief Set speed and direction of left motor **/

void motor_l_set(int direction, int speed);

/** @brief Set speed and direction of right motor **/

void motor_r_set(int direction, int speed);

/** @brief Turn the motors off **/

void motors_off(void);

/**@}**/ // end addtogroup

#endif

/**

 * Copyright (c) 2007 Colony Project

 * 

 * Permission is hereby granted, free of charge, to any person

 * obtaining a copy of this software and associated documentation

 * files (the "Software"), to deal in the Software without

 * restriction, including without limitation the rights to use,

 * copy, modify, merge, publish, distribute, sublicense, and/or sell

 * copies of the Software, and to permit persons to whom the

 * Software is furnished to do so, subject to the following

 * conditions:

 * 

 * The above copyright notice and this permission notice shall be

 * included in all copies or substantial portions of the Software.

 * 

 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,

 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES

 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND

 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT

 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,

 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING

 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR

 * OTHER DEALINGS IN THE SOFTWARE.

 **/

/**

 * @file i2c.c

 * @brief Implemenation of I2C communications protocol

 * 

 * In the case where you have master sends and then a master request to the same

 * address, you will not give up control of the line because the send and

 * request addresses are seen as different addresses. In between it will send a

 * restart but will not give up the line.

 *

 * @author CMU Robotics Club, Kevin Woo, Sursh Nidhiry

 * @bug Not tested.

 */

#include <avr/interrupt.h>

#include <util/twi.h>

#include "i2c.h"

#include "ring_buffer.h"

/**

 * @defgroup i2c I2C 

 *

 * @brief Provides Inter-Interconnected-Communications (I2C)

 * 

 * Initiates I2C functions on an ATMega128 which has a fully hardware Two Wire 

 * Interface (TWI) module. Any Atmel chip with this hardware should be able to

 * use the software.

 *

 * This code will operate in a multi-master enviornment and can be either a

 * slave or a master at any time (as long as they are not one or the other at

 * the moment. You can queue up multiple transmission modes in the buffer up to 

 * the buffer size. The buffer is implemented as a ring buffer.

 *

 * It is implemented using callback functions. Whenever you want to send a packet

 * you can call the built in send function (as a master) and it will send an array

 * of bytes. Master recieve and slave send/receive are all handled by the call back

 * functions. It is up to the end user to create functions that will handle the

 * receiving of packets. Their functions will be called with every byte recieved

 * so you must either buffer the inputs or handle each one separately.

 *

 * On errors we will simply flush the entire buffer.

 * 

 * For information on how I2C operates, read the wikipedia article

 * http://en.wikipedia.org/wiki/I2c

 * for a good explanation of how it works.

 * @{

 */

/** 

 * @brief Set bit rate 12 = 100kbit/s (max speed setting is 10 for an

 *  8 MHz clock). It is a divider, so the lower the number the faster the speed.

 */

#define I2C_BIT_RATE_DIVIDER 0x0C

static int start_flag;

static fun_mrecv_t master_recv_function;

static fun_srecv_t slave_recv_function;

static fun_send_t slave_send_function;

RING_BUFFER_NEW(i2c_buffer, 128, char, i2c_write_buff, i2c_addr_buff);

/**

 * @brief Initializes the i2c module.

 *

 * Initializes the I2C module to start listening on the i2c lines. If the callback functions

 * are not set to null they will be called when that transmission mode is called. The address

 * is your address that you will listen to when you are not the master.

 *

 * @param addr 			Your address on the I2C bus.

 * @param master_recv 	The address of the function to call when you receive a byte when you are a

 *                    	master.

 * @param slave_recv 	The address of the function to call when you are a slave you receive data

 *								from the master

 * @param slave_send		The address of the function to call when you are a slave and the master

 *								requests data from you.

 *

 * @return 0 for success, nonzero for failure

 **/

int i2c_init(char addr, fun_mrecv_t master_recv, fun_srecv_t slave_recv, fun_send_t slave_send) {

    master_recv_function = master_recv;

    slave_recv_function = slave_recv;

    slave_send_function = slave_send;

    RING_BUFFER_CLEAR(i2c_write_buff);

    RING_BUFFER_CLEAR(i2c_addr_buff);

    /* enables twi interrupt, automatic ack sending, and all twi hardware */

    TWCR =  (_BV(TWEA) | _BV(TWEN) | _BV(TWIE));

    /* sets the bit rate of data transmission */

    TWBR = I2C_BIT_RATE_DIVIDER;

    /* sets the address (it is stored in the 7 most significant bits) and allows

     * global messages to be accepted */

    TWAR = (addr << 1) | 1;

    return 0;

}

/**

 * @brief Sends a byte array over I2C as a master

 *

 * Will perform a send over I2C to the destination from data for the ammount of

 * bytes that bytes is.

 *

 * @param dest		Destination address of the data on the I2C bus.

 * @param data 	The pointer to the byte array of data

 * @param bytes	The amount of bytes long that the byte array is. This is how

 *						many bytes from the array that the function will send.

 *

 * @return zero for success, nonzero for failure

 **/

int i2c_send(char dest, char *data, unsigned int bytes) {

    int i;

    /* adding data to be sent to ring buffers is not atomic,

     * so disable interrupts */

    cli();

    for(i = 0; i < bytes; i++) {

        if(RING_BUFFER_FULL(i2c_write_buff)) {

            sei();

            return -1;

        }

        RING_BUFFER_ADD(i2c_write_buff, data[i]);

        RING_BUFFER_ADD(i2c_addr_buff, dest << 1);

    }

    /* re-enable the interrupts */

    sei();

    /* send the start bit, only if this device is not currently master */

    if(!start_flag) {

        start_flag = 1;

        TWCR |= _BV(TWSTA);

        TWCR |= _BV(TWINT);

    }

    return 0;

}

/**

 * @brief Send a master request to the destination

 *

 * Sends a request of data from the target address and calls

 * the callback function to handle data as it comes in. This function will

 * not work if the slave has not informationt to send or has nothing implemented

 * to send it.

 *

 * @param dest		The destination that we want to receive information from.

 *

 * @return 0 for success, nonzero for failure

 **/ 

int i2c_request(char dest) {

    if(RING_BUFFER_FULL(i2c_write_buff))

        return -1;

    RING_BUFFER_ADD(i2c_write_buff, 0);

    RING_BUFFER_ADD(i2c_addr_buff, (dest << 1) | 1);

    if(!start_flag) {

        start_flag = 1;

        TWCR |= _BV(TWSTA);

        TWCR |= _BV(TWINT);

    }

    return 0;

}

/** @} **/

/**

 * @brief Interrupt to handle I2C interrupts from the I2C hardware.

 * 

 * Uses the status codes from the I2C register to handle the events

 * needed to advance in I2C stages. For instance, you will get a bit for

 * receiving a start ack, then a address ack, then a data ack, etc.

 * The events are handled in each switch case. The status codes are defined

 * by avr-gcc in /util/twi.h but are the same codes as the Atmel documentation.

 *

 * Bytes are sent by popping off the ring buffer. It also will keep track

 * of what modes the send is in.

 *

 * Errors are handled here as well.

 **/ 

ISR(TWI_vect) {

	static char data_to_send;

	static char addr_to_send = -1;

	char addr, statusCode;

	//Get status code (only upper 5 bits)

	statusCode = (TWSR & 0xF8);

    switch (statusCode) {

        //Start sent successfully

        case TW_START:

        case TW_REP_START:

            /* Send address and write

             * ring_buffer will not be empty */

            RING_BUFFER_REMOVE(i2c_addr_buff, addr_to_send);

            RING_BUFFER_REMOVE(i2c_write_buff, data_to_send);

            /* first send the address */

            TWDR = addr_to_send; 

            //Turn off start bits

            TWCR &= ~_BV(TWSTA);

            break;

        //Master Transmit - Address sent succesfully

        case TW_MT_SLA_ACK:

        //Send byte

            TWDR = data_to_send;

            PORTG &= ~_BV(PG2);

            break;

        //Master Transmit - Data sent succesfully

        case TW_MT_DATA_ACK:    

            //If there is still data to send