Colony

// FIXME remove

typedef unsigned char uint8_t;

/**

 * 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 lights.h

 * @brief Contains declarations for managing the orbs.

 *

 * Contains declarations for using the orbs and PWM.

 *

 * @author Colony Project, CMU Robotics Club

 * Based on Firefly Library, by Tom Lauwers and Steven Shamlian

 **/

#ifndef _LIGHTS_H_

#define _LIGHTS_H_

/**

 * @addtogroup orbs

 * @{

 **/

/**

 * Quick start: call orb_init_pwm or orb_init_binary, depending on which mode you want to use. Call orb*set or

 * orb*set_color to set the orbs.

 * 

 * The orbs have two modes of operation: PWM mode and binary mode. In PWM mode, a pwm signal is generated by a hardware

 * timer and the orbs can be set to a value of 0 through 255. In binary mode, the orbs can only be turned on or off and

 * a value of 0 means "off" and any other value means "on". The mode can be chosen on initialization and can be changed

 * at runtime using the orb_set_mode function.

 *

 * Operation (PWM mode): On timer overflow, all LEDs with a value>0 are turned on and the output compare value for the

 * first LED is loaded. On compare match, the corresponding LED is turned off and the next output compare value is

 * loaded. All masks are precomputed and sorted by time when setting the values.

 *

 * The data structure (pwm_t) containing the PWM times and masks is triple buffered. This is because the buffer the ISR

 * is reading from may only be modified on timer overflow before the next PWM sequence is started, because otherwise the

 * next OCR value might be sed to a value smaller than the current timer value, resulting in the remaining channels not

 * being turned off in that PWM period (flash to on). When using two buffers, the page flip can only occur on a timer

 * overflow for the same reason. So after writing a buffer and marking it for page flip, neither of the buffers could be

 * modified because the front buffer is read by the ISR and the back buffer could be switched at any time. So the

 * calling thread would have to be delayed by up to one full PWM period (8ms in the current implementation, but

 * 20ms-50ms would be a reasonable value to expect here). To avoid this, triple buffering is used.

 *

 * The code for applying the orbs is fairly optimized. See the apply_orbs function for some time measurements and

 * further nodes.

 *

 * The PWM frequency is 120Hz (8ms period time). The next lower frequency (determined by the prescaler) is 30 Hz which

 * is too slow (orbs flicker).

 *

 * The orbs code is thread safe, which means that the functions may be called from another interrupt handler. If there

 * are multiple concurrent calls to the orb*set* functions, one of them is ignored and the orbs are never left in an

 * inconsistent state. For example, if the orbs are set to green by the main thread and to red by an interrupt handler,

 * the resulting color will be either red or green, but never yellow.

 * Thread safety is achieved by grabbing a lock at the beginning of all functions that modify the orb code and releasing

 * the lock at the end. If the lock is already taken, the function just returns doing nothing.

 *

 * Some performance measurements:

 *   - Time for setting new orb values (PWM mode):       35us-72us (depending on the degree to which the array is

 *                                                                  already in the correct order)

 *   - Time for setting new orb values (binary mode):        5.5us

 *

 *   - Interrupt time (PWM mode only):                         8us (overflow)

 *                                                            10us (output compare)

 *                                                             6us (last output compare)

 *                                                            30us (output compare, all value equal)

 *

 *   - Maximum total interrupt time per period:               64us

 *   - Maximum CPU usage for interrupts (PWM mode only):     <0.8%

 *

 *   - Maximum contiguous synchronized block:                 30us (output compare interrupt, all values equal)

 *

 * There are some potential optimizations left. See the source code for more information.

 *

 * A note on robustness: if the output compare interrupt is disabled for too long, either due to a long ISR or a long

 * synchronized code block, the orbs will flicker to brighter values for being turned off too late. With software PWM,

 * there's nothing at all to be done about that. The problem can be alleviated by using a lower PWM frequency, but then

 * the orbs will start flickering all the time due to the low update frequency.

 * Some measurements: with 100us synchronized blocks, the flickering is accepptably low. Longer synchronized blocks

 * mean more flickering. At 1ms synchronized blocks, the flickering is quite bad, especially for low orb values. Note

 * that orb value 0 never flickers at all because the corresponding channels are not turned on at all.

 * Test code (note the _delay_us restrictions!)

 *   orb_set (1,1,1); while (1) { SYNC { for (uint8_t m=0; m<10; ++m) { _delay_us(10); } } }

 **/

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

/**

 * @addtogroup orbs

 * @{

 **/

// ***** Predefined colors *****

/** @brief Red **/

#define RED       0xE0

/** @brief Orange **/

#define ORANGE    0xE4

/** @brief Yellow **/

#define YELLOW    0xE8

/** @brief Lime **/

#define LIME      0x68

/** @brief Green **/

#define GREEN     0x1C

/** @brief Cyan **/

#define CYAN      0x1F

/** @brief Blue **/

#define BLUE      0x03

/** @brief Pink **/

#define PINK      0xA6 

/** @brief Purple **/

#define PURPLE    0x41

/** @brief Magenta **/

#define MAGENTA   0xE3

/** @brief White **/

#define WHITE     0xFE

/** @brief Turn the orb off **/

#define ORB_OFF   0x00

// ***** Initialization *****

/** @brief Enables the orbs in default mode **/

void orb_init(void);

/** @brief Enables the orbs in binary mode **/

void orb_init_binary (void);

/** @brief Enables the orbs in PWM mode **/

void orb_init_pwm (void);

// ***** Mode setting *****

/** Specification of the orb mode **/

typedef uint8_t orb_mode_t;

/** @brief PWM mode **/

#define orb_mode_pwm 0

/** @brief Binary mode **/

#define orb_mode_binary 1

/** @brief Switches the orbs to the specified mode **/

void orb_set_mode (orb_mode_t mode);

/** @brief Disables the orb timer, but does not change the mode **/

void orb_disable_timer (void);

/** @brief Enables the orb timer, but does not change the mode **/

void orb_enable_timer (void);

// ***** Setting RGB colors *****

/** @brief set the specified orb to a specified color

* 	@deprecated This function indexes from 0 instead of 1 **/

void orb_n_set (uint8_t num, uint8_t red, uint8_t green, uint8_t blue);

/** @brief Set both orbs to a specified color **/

void orb_set(uint8_t red, uint8_t green, uint8_t blue);

/** @brief Set orb1 to a specified color **/

void orb1_set(uint8_t red_led, uint8_t green_led, uint8_t blue_led); 

/** @brief Set orb2 to a specified color **/

void orb2_set(uint8_t red_led, uint8_t green_led, uint8_t blue_led);

void orbs_set (uint8_t red1, uint8_t green1, uint8_t blue1, uint8_t red2, uint8_t green2, uint8_t blue2);

// ***** Settings predefined colors *****

/** @brief set the specified orb to the specified color

* 	@deprecated This function indexes from 0 instead of 1 **/

void orb_n_set_color(uint8_t num, uint8_t col);

/** @brief Set orb1 to a specified color **/

void orb1_set_color(uint8_t col);

/** @brief Set orb2 to a specified color **/

void orb2_set_color(uint8_t col);

/** @brief set the orbs to specified colors **/

void orbs_set_color(uint8_t col1, uint8_t col2);

/** @brief Set both orbs to a specified color **/

void orb_set_color(uint8_t col);

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

#endif

/**

 * @file spi.h

 * @brief Definitions for SPI

 * @author Colony Project, CMU Robotics Club

 **/

/**

 * @addtogroup spi

 * @{

 **/

#ifndef __SPI_H__

#define __SPI_H__

#define DOUBLE_SCK 1

#define SPR0_BIT 1

#define MASTER 1

#define SLAVE 0

#define MOSI _BV(PB2)

#define MISO _BV(PB3)

#define SS   _BV(PB0)

#define SCLK _BV(PB1)

typedef void (*spi_fun_recv_t)(char);

typedef void (*spi_fun_recv_complete_t)(void);

/** 

* @brief Initialize SPI

* 

* @param spi_fun_recv_t The function that handles SPI data, byte for byte.

* @param spi_fun_recv_complete_t  Called on a completed transmission - typically for cleaning up.

*/

void spi_init (spi_fun_recv_t, spi_fun_recv_complete_t);

/** 

* @brief Initialize SPI transfer.

* 

* @param char The number of bytes to transfer.

*/

void spi_transfer (char);

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

#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 time.h

 * @brief Contains time-related functions and definitions

 *

 * Contains functions and definitions for dealing with time,

 * namely delay_ms and the realtime clock.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#ifndef _TIME_H_

#define _TIME_H_

/*	Predefined times for prescale_opt in time.c.

	To make you own, know that a pulse is 1/16th of a second. You cannot get less than this. To get more, you need

	to know how many 16ths of a second are in the time you want. (Time_desired * 16 = prescaler_opt)

*/

/**

 * @addtogroup time

 * @{

 **/

/** @brief A sixteenth of a second **/

#define SIXTEENTH_SECOND 1

/** @brief An eighth of a second **/

#define EIGTH_SECOND 2

/** @brief A quarter of a second **/

#define QUARTER_SECOND 4

/** @brief Half of a second **/

#define HALF_SECOND	8

/** @brief One second **/

#define SECOND 16

/** @brief Two seconds **/

#define TWO_SECOND 32

/** @brief Four seconds **/

#define FOUR_SECOND 64

/** @brief Delay execution for the specified time **/

void delay_ms(int ms) ;

/** @brief Enable the realtime clock **/

void rtc_init(int prescale_opt, void (*rtc_func)(void));

/** @brief Reset the counter of the realtime clock **/

void rtc_reset(void);

/** @brief Get the value of the realtime clock. **/

int rtc_get(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.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 analog.h

 * @brief Contains functions and definitions for using the ADC

 * 

 * Contains definitions and function prototypes for using the

 * ADC to detect analog signals on pins AN0 - AN7.

 * AN6 and AN7 are used for the wheel and battery.

 * The pins labeled E6 and E7 are external interrupt pins and are not related 

 * to analog.

 * @author Colony Project, CMU Robotics Club, based on firefly

 * originally taken from fwr analog file (author: Tom Lauwers)

 * loop code written by Kevin Woo and James Kong

 */

#ifndef _ANALOG_H

#define _ANALOG_H

#include <inttypes.h>

/**

 * @addtogroup analog

 * @{

 **/

/** @brief Analog port 0 **/

#define AN0 0x00

/** @brief Analog port 1 **/

#define AN1 0x01

/** @brief Analog port 2 **/

#define AN2 0x02

/** @brief Analog port 3 **/

#define AN3 0x03

/** @brief Analog port 4 **/

#define AN4 0x04

/** @brief Analog port 5 **/

#define AN5 0x05

/** @brief Analog port 6 **/

#define AN6 0x06

/** @brief Analog port 7 **/

#define AN7 0x07

/** @brief Analog port 8 **/

#define AN8 0x08

/** @brief Analog port 9 **/

#define AN9 0x09

/** @brief Analog port 10 **/

#define AN10 0x0A

/** @brief Analog port 11 **/

#define AN11 0x0B

/** @brief Analog port 12 **/

#define AN12 0x0C

/** @brief Analog port 13 **/

#define AN13 0x0D

/** @brief Analog port 14 **/

#define AN14 0x0E

/** @brief Analog port 15 **/

#define AN15 0x0F

/** @brief BOM_PORT analog port for BOM **/

#define BOM_PORT AN0

/** @brief EXT_MUX analog port **/

#define EXT_MUX AN7

/** @brief Analog port for the wheel **/

#define WHEEL_PORT AN10

/** @brief Analog port for the battery voltage detector **/

#define BATT_PORT  AN11

/** @brief Analog loop status. ADC conversion running. **/

#define ADC_LOOP_RUNNING 1

/** @brief Analog loop status.  No ADC conversion running.**/

#define ADC_LOOP_STOPPED 0

/** @brief Analog init parameter. Start the analog loop. **/

#define ADC_START 1

/** @brief Analog init parameter. Don't start the analog loop. **/

#define ADC_STOP  0

#define ADMUX_OPT 0x60

/** @brief Struct to hold the value of a particular analog port **/

typedef struct {

  uint8_t adc8;

  uint16_t adc10;

} adc_t;

/** @brief Initialize analog ports. Will start running a loop

    if start_conversion is ADC_START.**/

void analog_init(int start_conversion);

/** @brief starts the analog loop. Doesn't do anything if the loop is already running. **/

void analog_start_loop(void);

/** @brief Stops the analog loop. Doesn't do anything if the loop is already stopped. **/

void analog_stop_loop(void);

/** @brief Returns the status of the analog loop. **/

int analog_loop_status(void);

/** @brief Returns an 8-bit analog value from the look up table. Use this instead of analog_get8. **/

unsigned int analog8(int which);

/** @brief Returns an 10-bit analog value from the look up table. Use this instead of analog_get10. **/

unsigned int analog10(int which);

/** @brief Read the position of the wheel. **/

int wheel(void);

/** @brief Read an 8-bit number from an analog port. Loop must be stopped for this to work. **/

unsigned int analog_get8(int which);

/** @brief Read a 10-bit number from an analog port. Loop must be stopped for this to work. **/

unsigned int analog_get10(int which);

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

#endif

/**

 * 

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

	/** @brief Left wheel **/

	#define LEFT 0

#endif

#ifndef RIGHT

	/** @brief Right wheel **/

	#define RIGHT 1

#endif

/** @brief Max value of valid encoder reading. **/

#define ENCODER_MAX 1024

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

/** @brief 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)

/** @brief Buffer size **/

#define BUFFER_SIZE 46

#define ERR_VEL 1024

/** @brief Initialize encoders. **/

void encoders_init(void);

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

int encoder_read(char encoder);

/** @brief Get total distance traveled.

 *  @note  Simply calls encoder_get_dx.

 **/

int encoder_get_x(char encoder);

/** @brief Get instantaneous velocity. **/

int encoder_get_v(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 Waits for the next n encoder reading, then returns. **/

void encoder_wait( int nReadings );

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

#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 lcd.h

 * @brief Contains definitions for dealing with the LCD screen.

 * 

 * Contains definitions and functions for dealing with the 

 * LCD screen.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#ifndef _LCD_H_

#define _LCD_H_

/**

 * @addtogroup lcd

 * @{

 **/

/** @brief Initialize the LCD screen **/

void lcd_init(void);

/** @brief Clear the LCD screen **/

void lcd_clear_screen( void );

/** @brief Print a char to the LCD screen **/

void lcd_putc(char c);

/** @brief Print a string to the LCD screen **/

void lcd_puts(char *s);

/** @brief Print an int to the LCD screen **/

void lcd_puti(int value);

/** @brief Set the current cursor position **/

void lcd_gotoxy(int x, int y);

/** @} **/

#endif

/**

 * @file odometry.h

 * @brief Code for estimating the robots pose.

 * 

 * Offers simple position and orientation information.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#ifndef __ODOMETRY_C__

#define __ODOMETRY_C__

/**

 * @addtogroup odometry 

 * @{

 **/

//Odometry resolution, *64 microseconds.

#define ODOMETRY_CLK 255u 

#define TIME_SCALE 64

//Wheel = 2.613 in.  

//Circumference = 208.508133 mm

//Distance per encoder click (circumference / 1024)  = 203.621224 um.

//Robot width = 5.3745 in. = 136.5123 mm

#define ROBOT_WIDTH_UM 137000  //um

#define CLICK_DISTANCE_UM 204 //um

#define DISTANCE_SCALE 2.10526316 //Magic constant.

#define ANGLE_SCALE 1.12823207 //Magic constant.

/** @brief Retrieve the robots estimated x position*/

long odometry_dx(void);

/** @brief Retrieve the robots estimated y position*/

long odometry_dy(void);

/** @brief Retrieve the robots estimated orientation*/

double odometry_angle(void);

/** @brief Initialize odometry. MUST be called before 

 * the other functions work.**/

void odometry_init(void);

/** @brief Reset position and orientation to the origin facing

 * the x axis.*/

void odometry_reset(void);

/** @brief Report estimated velocity [mm/s].*/

long odometry_velocity(void);

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

#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 move.h

 * @brief Contains definitions for controlling robot motion

 * 

 * This file offers higher-level functions for robot motion.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#ifndef _MOVE_H_

#define _MOVE_H_

/**

 * @addtogroup move

 * @{

 **/

/** @brief Move forwards **/

#define FWD 0

/** @brief Move backwards **/

#define BCK 1

/** @brief A slow speed **/

#define SLOW_SPD 96

/** @brief Half of the full speed **/

#define HALF_SPD 128

/** @brief A normal speed **/

#define NRML_SPD 160

/** @brief A fast speed **/

#define FAST_SPD 192

/** @brief The maximum speed **/

#define FULL_SPD 255

/** @brief A slow turning speed **/

#define SLOW_TURN 64

/** @brief A medium turning speed **/

#define NRML_TURN 96

/** @brief A high turning speed **/

#define FAST_TURN 128

/** @brief Move the robot at the specified velocity **/

void move(int velocity, int omega);

/** @brief Move the robot while avoiding obstacles **/

void move_avoid(int velocity, int omega, int strength);

/** @} **/

#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 battery.h

 * @brief Definitions for checking battery voltage.

 * 

 * Contains definitions for checking the voltage of the 

 * battery.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#ifndef _BATTERY_H_

#define _BATTERY_H_

/**

 * @addtogroup battery

 * @{

 **/

/** @brief Normal battery voltage. (6 V) **/

#define BATTERY_NORMALV 154

/** @brief Charging battery voltage. (7 V) **/

#define BATTERY_CHARINGV 179

/** @brief Low battery voltage. ( < 6 V) **/

#define BATTERY_LOWV 152

/** @brief Read the battery voltage. **/

int battery8(void);

/** @brief Read the battery voltage in deciVolts. **/

int battery(void);

/** @brief Check if the battery is low. **/

char battery_low(void);

/** @brief Get an average battery voltage reading. **/

int battery8_avg(int n_samples);

/** @} **/ //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 dio.h

 * @brief Definitions for digital input / output

 *

 * This file contains definitions and functions for dealing

 * with digital input and output.

 *

 * @author Colony Project, CMU Robotics Club

 * Based on Tom Lauwer's Firefly Library

 **/

#ifndef _DIO_H

#define _DIO_H

/*

  these are ALL the pins

  don't use unless you know what you're doing

*/

/*

  DIO pins on new dragonfly boards are shown below:

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

  | _PIN_E6 | _PIN_E7 | _PIN_D2 | _PIN_D3 | VCC |

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

  | _PIN_E2 | _PIN_E3 | _PIN_E4 | _PIN_E5 | GND |

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

*/

/** @brief Port A **/

#define _PORT_A 1

/** @brief Port B **/

#define _PORT_B 2

/** @brief Port C **/

#define _PORT_C 3

/** @brief Port D **/

#define _PORT_D 4

/** @brief Port E **/

#define _PORT_E 5

/** @brief Port F **/

#define _PORT_F 6

/** @brief Port G **/

#define _PORT_G 7

/** @brief Pin A0 **/

#define _PIN_A0 8

/** @brief Pin A1 **/

#define _PIN_A1 9

/** @brief Pin A2 **/

#define _PIN_A2 10

/** @brief Pin A3 **/

#define _PIN_A3 11

/** @brief Pin A4 **/

#define _PIN_A4 12

/** @brief Pin A5 **/

#define _PIN_A5 13

/** @brief Pin A6 **/

#define _PIN_A6 14 

/** @brief Pin A7 **/

#define _PIN_A7 15

/** @brief Pin B0 **/

#define _PIN_B0 16

/** @brief Pin B1 **/

#define _PIN_B1 17

/** @brief Pin B2 **/

#define _PIN_B2 18

/** @brief Pin B3 **/

#define _PIN_B3 19

/** @brief Pin B4 **/

#define _PIN_B4 20

/** @brief Pin B5 **/

#define _PIN_B5 21

/** @brief Pin B6 **/

#define _PIN_B6 22

/** @brief Pin B7 **/

#define _PIN_B7 23

/** @brief Pin C0 **/

#define _PIN_C0 24

/** @brief Pin C1 **/

#define _PIN_C1 25

/** @brief Pin C2 **/

#define _PIN_C2 26

/** @brief Pin C3 **/

#define _PIN_C3 27

/** @brief Pin C4 **/

#define _PIN_C4 28

/** @brief Pin C5 **/

#define _PIN_C5 29

/** @brief Pin C6 **/

#define _PIN_C6 30

/** @brief Pin C7 **/

#define _PIN_C7 31

/** @brief Pin D0 **/

#define _PIN_D0 32  // pin DIO6 on new dragonfly boards

/** @brief Pin D1 **/

#define _PIN_D1 33  // pin DIO7

/** @brief Pin D2 **/

#define _PIN_D2 34

/** @brief Pin D3 **/

#define _PIN_D3 35

/** @brief Pin D4 **/

#define _PIN_D4 36

/** @brief Pin D5 **/

#define _PIN_D5 37

/** @brief Pin D6 **/

#define _PIN_D6 38

/** @brief Pin D7 **/

#define _PIN_D7 39

/** @brief Pin E0 **/

#define _PIN_E0 40

/** @brief Pin E1 **/

#define _PIN_E1 41

/** @brief Pin E2 **/

#define _PIN_E2 42  // pin DIO0

/** @brief Pin E3 **/

#define _PIN_E3 43  // pin DIO1

/** @brief Pin E4 **/

#define _PIN_E4 44  // pin DIO2

/** @brief Pin E5 **/

#define _PIN_E5 45  // pin DIO3

/** @brief Pin E6 **/

#define _PIN_E6 46  // pin DIO4

/** @brief Pin E7 **/

#define _PIN_E7 47  // pin DIO5

/** @brief Pin F0 **/

#define _PIN_F0 48

/** @brief Pin F1 **/

#define _PIN_F1 49

/** @brief Pin F2 **/

#define _PIN_F2 50

/** @brief Pin F3 **/

#define _PIN_F3 51

/** @brief Pin F4 **/

#define _PIN_F4 52

/** @brief Pin F5 **/

#define _PIN_F5 53

/** @brief Pin F6 **/

#define _PIN_F6 54

/** @brief Pin F7 **/

#define _PIN_F7 55

/** @brief Pin G0 **/

#define _PIN_G0 56

/** @brief Pin WR **/

#define _PIN_WR 56

/** @brief Pin G1 **/

#define _PIN_G1 57

/** @brief Pin RD **/

#define _PIN_RD 57

/** @brief Pin G2 **/

#define _PIN_G2 58

/** @brief Pin ALE **/

#define _PIN_ALE 58

/** @brief Pin G3 **/

#define _PIN_G3 59

/** @brief Pin TOSC2 **/

#define _PIN_TOSC2 59

/** @brief Pin G4 **/

#define _PIN_G4 60

/** @brief Pin TOSC1 **/

#define _PIN_TOSC1 60

//#define _PIN_G5 61

//#define _PIN_G6 62

//#define _PIN_G7 63

/*

  These are the header pins (the ones you can connect things to)

  Feel free to use these

*/

/**

 * @addtogroup dio

 * @{

 **/

/** @brief Pin A0 **/

#define PIN_A0 8

/** @brief Pin A1 **/

#define PIN_A1 9

/** @brief Pin A2 **/

#define PIN_A2 10

/** @brief Pin A3 **/

#define PIN_A3 11

/** @brief Pin A4 **/

#define PIN_A4 12

/** @brief Pin A5 **/

#define PIN_A5 13

/** @brief Pin A6 **/

#define PIN_A6 14 

/** @brief Pin A7 **/

#define PIN_A7 15

/** @brief Pin SS **/

#define PIN_SS 16

/** @brief Pin SCK **/

#define PIN_SCK 17

/** @brief Pin MOSI **/

#define PIN_MOSI 18

/** @brief Pin MISO **/

#define PIN_MISO 19

/** @brief LCD Command Pin **/

#define PIN_LCD_COMMAND 20

/** @brief Pin C0 **/

#define PIN_C0 24

/** @brief Pin C1 **/

#define PIN_C1 25

/** @brief Pin C2 **/

#define PIN_C2 26

/** @brief Pin C3 **/

#define PIN_C3 27

/** @brief Pin C4 **/

#define PIN_C4 28

/** @brief Pin C5 **/

#define PIN_C5 29

/** @brief Pin C6 **/

#define PIN_C6 30

/** @brief Pin C7 **/

#define PIN_C7 31

/** @brief Pin SCL **/

#define PIN_SCL 32

/** @brief Pin SDA **/

#define PIN_SDA 33

/** @brief Pin RX0 **/

#define PIN_RX0 40

/** @brief Pin TX0 **/

#define PIN_TX0 41

/** @brief LCD Reset Pin **/

#define PIN_LCD_RESET 42

/** @brief Pin E6 **/

#define PIN_E6 46

/** @brief Pin EXT_DIO1 **/

#define PIN_EXT_DIO1 46

/** @brief Pin E7 **/

#define PIN_E7 47

/** @brief Pin EXT_DIO2 **/

#define PIN_EXT_DIO2 48

/** @brief Pin AN0 **/

#define PIN_AN0 48

/** @brief Pin ADC0 **/

#define PIN_ADC0 48

/** @brief Pin AN1 **/

#define PIN_AN1 49

/** @brief Pin ADC1 **/

#define PIN_ADC1 49

/** @brief Pin AN2 **/

#define PIN_AN2 50

/** @brief Pin ADC2 **/

#define PIN_ADC2 50

/** @brief Pin AN3 **/

#define PIN_AN3 51

/** @brief Pin ADC3 **/

#define PIN_ADC3 51

/** @brief Pin AN4 **/

#define PIN_AN4 52

/** @brief Pin ADC4 **/

#define PIN_ADC4 52

/** @brief Pin AN5 **/

#define PIN_AN5 53

/** @brief Pin ADC5 **/

#define PIN_ADC5 53

/** @brief Pin AN6 **/

#define PIN_AN6 54

/** @brief Pin ADC6 **/

#define PIN_ADC6 54

/** @brief Pin AN7 **/

#define PIN_AN7 55

/** @brief Pin ADC7 **/

#define PIN_ADC7 55

/** @brief Wheel Pin **/

#define PIN_WHEEL 54

/** @brief Battery Voltage Monitor Pin **/

#define PIN_BATT 55

/** @brief button1 Pin **/

#define PIN_BTN1 56

/** @brief button2 Pin **/

#define PIN_BTN2 57

/** @brief LED1 Pin **/

#define PIN_LED1 58

/* Buttons */

/** @brief Button Pin **/

#define PIN_BTN PING

/** @brief button2 Pin **/

#define BTN2 PING1

/** @brief button1 Pin **/

#define BTN1 PING0

/** @brief Read a portpin. **/

int digital_input(int);

/** @brief Output to a portpin. **/

void digital_output(int bit, int val);

/** @brief Pullup a portpin. **/

void digital_pull_up(int);

/** @brief Check if button1 is pressed. **/

int button1_read( void );

/** @brief Check if button1 is clicked. **/

int button1_click( void );

/** @brief Wait until button1 is pressed. **/

void button1_wait( void );

/** @brief Check if button2 is pressed. **/

int button2_read( void );

/** @brief Check if button2 is clicked. **/

int button2_click( void );

/** @brief Wait until button2 is pressed. **/

void button2_wait( 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 dragonfly_lib.h

 * @brief Contains other include files

 * 

 * Include this file for all the functionality of libdragonfly.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#ifndef _DRAGONFLY_LIB_H_

#define _DRAGONFLY_LIB_H_

/**

 * @addtogroup dragonfly

 * @{

 **/

// Configuration definitions

/** @brief Initialize analog **/

#define ANALOG 0x01

/** @brief Initialize serial communications **/

#define SERIAL 0x02

/** @brief Initialize USB communications **/

#define USB    0x02

/** @brief Initialize communications **/

#define COMM   0x02

/** @brief Initialize the orb **/

#define ORB    0x04

/** @brief Initialize the motors **/

#define MOTORS 0x08

/** @brief Initialize I2C **/

#define I2C    0x20

/** @brief Initialize the buzzer **/

#define BUZZER 0x40

/** @brief Initialize the LCD screen **/

#define LCD    0x80

/** @brief Initialize the rangefinders **/

#define RANGE  0x0100

/** @brief Initialize the BOM **/

#define BOM  0x0200

/** @brief Initilize encoders **/

#define ENCODERS 0x400

/** @brief Initialize everything  **/

#define ALL_ON 0x07FF

/** @brief Initialize the board **/

void dragonfly_init(int config);

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

#include <inttypes.h>

#include <avr/io.h>

#include <avr/interrupt.h>

#include <util/delay.h>

#include <util/twi.h>

// This file is included from the libdragonfly directory because it seems to be

// missing from the AVR libc distribution.

#include "atomic.h"

#include "analog.h"

#include "dio.h"

#include "time.h"

#include "lcd.h"

#include "lights.h"

#include "motor.h"

#include "serial.h"

#include "buzzer.h"

#include "rangefinder.h"

#include "bom.h"

#include "encoders.h"

#include "move.h"

#include "reset.h"

#include "math.h"

#include "eeprom.h"

#include <stddef.h>

#include <stdbool.h>

/** @brief shortcut for ATOMIC_BLOCK(ATOMIC_RESTORESTATE) **/

#define SYNC ATOMIC_BLOCK(ATOMIC_RESTORESTATE)

/** @brief atomically grab a lock if it is free, return otherwise **/

#define REQUIRE_LOCK_OR_RETURN(LOCK) do { SYNC { if (LOCK) return; LOCK=1; } } while (0)

/** @brief atomically release a lock **/

#define RELEASE_LOCK(LOCK) do { LOCK=0; } while (0)

#endif

/* This file is in the dragonfly library directory because it seems to be

   missing from the AVR libc distribution */

/* Copyright (c) 2007 Dean Camera

   All rights reserved.

   Redistribution and use in source and binary forms, with or without

   modification, are permitted provided that the following conditions are met:

   * Redistributions of source code must retain the above copyright

     notice, this list of conditions and the following disclaimer.

   * Redistributions in binary form must reproduce the above copyright

     notice, this list of conditions and the following disclaimer in

     the documentation and/or other materials provided with the

     distribution.

   * Neither the name of the copyright holders nor the names of

     contributors may be used to endorse or promote products derived

     from this software without specific prior written permission.

  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

  POSSIBILITY OF SUCH DAMAGE.

*/

/* $Id: atomic.h,v 1.3 2007/12/20 14:17:56 joerg_wunsch Exp $ */

#ifndef _UTIL_ATOMIC_H_

#define _UTIL_ATOMIC_H_ 1

#include <avr/io.h>

#include <avr/interrupt.h>

#if !defined(__DOXYGEN__)

/* Internal helper functions. */

static __inline__ uint8_t __iSeiRetVal(void)

{

    sei();

    return 1;

}

static __inline__ uint8_t __iCliRetVal(void)

{

    cli();

    return 1;

}

static __inline__ void __iSeiParam(const uint8_t *__s)

{

    sei();

    __asm__ volatile ("" ::: "memory");

    (void)__s;

}

static __inline__ void __iCliParam(const uint8_t *__s)

{

    cli();

    __asm__ volatile ("" ::: "memory");

    (void)__s;

}

static __inline__ void __iRestore(const  uint8_t *__s)

{

    SREG = *__s;

    __asm__ volatile ("" ::: "memory");

}