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 ligths.c

 * @brief Orbs

 *

 * Implemenation for the orbs (tri-colored LEDs)

 *

 * @author Colony Project, CMU Robotics Club

 * @bug Colors are incorrect, seems to not work with wireless library

 **/

/*

lights.c

Controls orb1 and orb2. Also contains the framework for a software PWM that may be used for servos in the future.

author: CMU Robotics Club, Colony Project

Change Log:

2.4.07 - Aaron

	Revamped orb code so it works.  Need to check interaction with rtc, and tweak some colors.

2.1.07 - James

	Modified sort_buffer() to prune for repeats.  PWM now uses orb_buf_size for the number of orb values in orb_time_arr[].

		Changed sorting algorithm used in sort_buffer() to selection sort (faster). And it works now.

1.25.07 - KWoo

	Deleted old FF+ code to make it cleaner. Commented code. This all works. Note however that if you ever plan to use the

		software PWM (which is this) you will need to change the implementation of orb_enable() and orb_disable() to not

		shutdown the PWM.

*/

#include "lights.h"

#include "dragonfly_lib.h"

#include <avr/interrupt.h>

#define ORB_RESET 1025

#define ORBPORT PORTC

#define ORBDDR DDRC

#define ORBMASK 0x77

/***** Port and Pin Definitions ****/

//Orb Ports and Registers

#define ORB_PORT        PORTC

#define ORB_DDR         DDRC

//Orb Pins

#define ORB1_RED        0x00

#define ORB1_GREEN      0x01

#define ORB1_BLUE       0x02

#define ORB2_RED        0x04

#define ORB2_GREEN      0x05

#define ORB2_BLUE       0x06

#define ORB_COUNT 8	//please dont change this, or bad things might happen

// an orb node

struct ORB_NODE {

   uint8_t num;

   uint16_t angle;

};

//the change in an orb

struct ORB_CHANGE {

   uint16_t port_val;

   uint16_t split_time_period;

};

// the status of an orb

struct ORB_STATUS_STRUCT {

   struct ORB_NODE orbs[ORB_COUNT];

   uint16_t orb_angles[ORB_COUNT];

   struct ORB_CHANGE changes[ORB_COUNT+1];

   uint8_t change_count;

   uint8_t new_angles;

   uint8_t current_orb;

} ORB_STATUS;

//TODO: if these are only used in this file, make them static.  If they are used

// by other files then put them in a header file

void orb_sort(void);

void orb_setup_pulse(void);

SIGNAL (SIG_OUTPUT_COMPARE3C){

		//pull the correct ones down

      ORBPORT &= (~ORBMASK)|(ORB_STATUS.changes[ORB_STATUS.current_orb].port_val);

      ++ORB_STATUS.current_orb; //now look at next orb transition

      if (ORB_STATUS.current_orb < ORB_STATUS.change_count) { //if it isnt the end...

			//setup timer for next pull down

         OCR3C = TCNT3+ORB_STATUS.changes[ORB_STATUS.current_orb].split_time_period;

     }

      else { //we are done with these pulses

		orb_setup_pulse();

      }

}

//sets a channel to a value

void orb_set_angle(int orb, int angle) {

	uint8_t mysreg;

	orb=orb&0x07; //only have 8

	angle=angle&0xff; //only accept 0-255

	angle=255-angle; //inverse intensity

	angle=angle<<2; //scale up so that we dont run it too often

	angle+=3; //0 values dont really work

   if (ORB_STATUS.orb_angles[orb] != angle) { //if the angle has changed

	  mysreg=SREG; 

	  cli(); //disable interrupts

      ORB_STATUS.orb_angles[orb] = angle; //update angle

      ORB_STATUS.new_angles = 1;

	  SREG=mysreg; //put interrupt status back

   }

}

void orb_sort(void) {

   int done = 0, i;

   while (! done) {

      done = 1;

      for (i = 0; i < ORB_COUNT - 1; ++i) {  //loop through all

			//if they are out of order, swap them

         if (ORB_STATUS.orbs[i].angle > ORB_STATUS.orbs[i+1].angle) {

            ORB_STATUS.orbs[i].angle ^= ORB_STATUS.orbs[i+1].angle;

            ORB_STATUS.orbs[i+1].angle ^= ORB_STATUS.orbs[i].angle;

            ORB_STATUS.orbs[i].angle ^= ORB_STATUS.orbs[i+1].angle;

            ORB_STATUS.orbs[i].num ^= ORB_STATUS.orbs[i+1].num;

            ORB_STATUS.orbs[i+1].num ^= ORB_STATUS.orbs[i].num;

            ORB_STATUS.orbs[i].num ^= ORB_STATUS.orbs[i+1].num;

            done = 0;

         }

      }

   }

}

//calculate the split times

void orb_setup_pulse(void) {

   int i;

   uint16_t my_port;

   uint16_t sum = 0;

   uint16_t split_time;

   my_port = 0xff; //all on

   if (ORB_STATUS.new_angles) {

      ORB_STATUS.change_count = 0;

	  for (i = 0; i < ORB_COUNT; ++i) { //get the new values

         ORB_STATUS.orbs[i].angle = ORB_STATUS.orb_angles[ORB_STATUS.orbs[i].num];

      }

      orb_sort(); //sort them

      ORB_STATUS.new_angles = 0;

      for (i = 0; i < ORB_COUNT; ++i) { //calculate split times

         split_time = ORB_STATUS.orbs[i].angle - sum;

         my_port &= ~_BV(ORB_STATUS.orbs[i].num);

         for (; i < ORB_COUNT - 1 && ORB_STATUS.orbs[i].angle == ORB_STATUS.orbs[i+1].angle; ++i) {

            my_port &= ~_BV(ORB_STATUS.orbs[i+1].num); //look for doups

         }

         ORB_STATUS.changes[ORB_STATUS.change_count].port_val = my_port; //which pins are low

         ORB_STATUS.changes[ORB_STATUS.change_count].split_time_period = split_time;

		 ++ORB_STATUS.change_count;

         sum += split_time;

      }

      ORB_STATUS.changes[ORB_STATUS.change_count].port_val = my_port;

      ORB_STATUS.changes[ORB_STATUS.change_count].split_time_period = ORB_RESET - sum; //get a constant period

      ++ORB_STATUS.change_count;

   }

   ORB_STATUS.current_orb = 0;

    ORBPORT |= ORBMASK; //start with all high

	OCR3C = TCNT3 + ORB_STATUS.changes[0].split_time_period; //wait for first split

}

/**

 * @defgroup orbs Orbs

 * @brief Functions for controlling the color of the orbs.

 * 

 * Functions for controlling the color and lighting of the orbs.

 *

 * @{

 **/

/**

 * Initializes the PWM for Orb control. This must be called before 

 * the orbs are used for them to function.

 **/

void orb_init() 

{	

   int i;

   uint8_t mysreg;

   ORBDDR |= ORBMASK;	//all outputs

	mysreg=SREG;

	cli(); //turn off interrupts for now

	//init everything

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

      ORB_STATUS.orbs[i].num = i;

      ORB_STATUS.orbs[i].angle = 1023;	//127 is a pretty stupid start angle, but oh well

      ORB_STATUS.orb_angles[i] = 1023;

   }

   ORB_STATUS.new_angles = 1;

   ORB_STATUS.change_count = 0;

	//init timer3

	TCCR3A = 0; 

	TCCR3B = _BV(CS31); //prescale = 8

	TCCR3C = 0;

	ETIMSK |= _BV(OCIE3C); //turn on oc3c interrupt

	OCR3C = TCNT3+ORB_RESET;

	SREG=mysreg;

}

/**

 * Set both orbs to the color specified. orb_init must

 * be called before this function may be used.

 *

 * @param red_led the red component of the color

 * @param green_led the green component of the color

 * @param blue_led the blue component of the color

 *

 * @see orb_init

 **/

void orb_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {

	orb1_set(red_led,green_led,blue_led);

	orb2_set(red_led,green_led,blue_led);

}

/**

 * Set orb1 to the color specified. orb_init must

 * be called before this function may be used.

 *

 * @param red_led the red component of the color

 * @param green_led the green component of the color

 * @param blue_led the blue component of the color

 *

 * @see orb_init

 **/

void orb1_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {

	orb_set_angle(0,red_led);

	orb_set_angle(1,green_led);

	orb_set_angle(2,blue_led);

}

/**

 * Set orb2 to the color specified. orb_init must

 * be called before this function may be used.

 *

 * @param red_led the red component of the color

 * @param green_led the green component of the color

 * @param blue_led the blue component of the color

 *

 * @see orb_init

 **/

void orb2_set(unsigned char red_led, unsigned char green_led, unsigned char blue_led) {	

	orb_set_angle(4,red_led);

	orb_set_angle(5,green_led);

	orb_set_angle(6,blue_led);

}

/**

 * Set both orbs to the specified color. This function

 * is intended to be used with the predefined

 * colors. orb_init must be called before this

 * function may be used.

 *

 * @param col the color to set the orbs to

 *

 * @see orb_init

 **/

void orb_set_color(int col)

{

 int red, green, blue;

 red = ((col & 0xE0) >> 5) * 36;

 green = ((col & 0x1C) >> 2) * 36;

 blue = (col & 0x03) * 85;

 orb_set(red, green, blue);

}

/**

 * Set orb1 to the specified color. This function

 * is intended to be used with the predefined

 * colors. orb_init must be called before this

 * function may be used.

 *

 * @param col the color to set the orbs to

 *

 * @see orb_init

 **/

void orb1_set_color(int col)

{

 int red, green, blue;

 red = ((col & 0xE0) >> 5) * 36;

 green = ((col & 0x1C) >> 2) * 36;

 blue = (col & 0x03) * 85;

 orb1_set(red, green, blue);

}

/**

 * Set orb2 to the specified color. This function

 * is intended to be used with the predefined

 * colors. orb_init must be called before this

 * function may be used.

 *

 * @param col the color to set the orbs to

 *

 * @see orb_init

 **/

void orb2_set_color(int col)

{

 int red, green, blue;

 red = ((col & 0xE0) >> 5) * 36;

 green = ((col & 0x1C) >> 2) * 36;

 blue = (col & 0x03) * 85;

 orb2_set(red, green, blue);

}

//DOES THIS WORK?

// Disables the timer1 interrupt, disabling the Orb's color fading capabilities

// You can still turn the red, green, and blue leds on and off with set_orb_dio

/* If we use the PWM for anything else besides the ORB, this implementation needs to be done better */

/**

 * Disables the orb color fading capabilities

 * by disabling the timer1 interrupt.

 *

 * @see orb_init

 **/

void orb_disable()

{

	TCCR3B &= 0;  	//Turn off everything

	ORB_PORT |= _BV(ORB1_RED);

	ORB_PORT |= _BV(ORB1_GREEN);

	ORB_PORT |= _BV(ORB1_BLUE);

	ORB_PORT |= _BV(ORB2_RED);

	ORB_PORT |= _BV(ORB2_GREEN);

	ORB_PORT |= _BV(ORB2_BLUE);

}

//DOES THIS WORK?

// Enables the timer1 interrupt, enabling the Orb's color fading capabilities

/**

 * Enables the orb's color fading capabilities.

 *

 * @see orb_init

 **/

void orb_enable()

{

//	TCCR0 |= _BV(COM01) | _BV(COM00)  | _BV(WGM00) | _BV(CS01);	//Toggle OC Pin on match, FAST PWM Mode, clock/8

	TCCR3B =_BV(CS31);

}

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

/**

 * 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.c

 * @brief LCD

 *

 * Implementation of functions for using the LCD.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#include <avr/io.h>

#include <lcd.h>

#include <time.h>

//LCD defines

#define RST _BV(4)  // pd4 (GPIO)

#define SCE _BV(0)  // pb0 (~SS)

#define D_C _BV(5)  // pd5 (GPIO?)

#define SDI _BV(2)  // pb2 (MOSI)

#define SCK _BV(1) // pb1 (SCK)

#define LCDPORT PORTB

#define LCDDDR DDRB

#define LCDRESETPORT PORTD

#define LCDRESETDDR DDRD

void lcd_putbyte(unsigned char b);

//**************************************Old shit below!*****************

/*

FontLookup - a lookup table for all characters

*/

static const unsigned char FontLookup [][5] =

{

    { 0x00, 0x00, 0x00, 0x00, 0x00 },  // sp

    { 0x00, 0x00, 0x5f, 0x00, 0x00 },   // !

    { 0x00, 0x07, 0x00, 0x07, 0x00 },   // "

    { 0x14, 0x7f, 0x14, 0x7f, 0x14 },   // #

    { 0x24, 0x2a, 0x7f, 0x2a, 0x12 },   // $

    { 0x23, 0x13, 0x08, 0x64, 0x62 },   // %

    { 0x36, 0x49, 0x55, 0x22, 0x50 },   // &

    { 0x00, 0x05, 0x03, 0x00, 0x00 },   // '

    { 0x00, 0x1c, 0x22, 0x41, 0x00 },   // (

    { 0x00, 0x41, 0x22, 0x1c, 0x00 },   // )

    { 0x14, 0x08, 0x3E, 0x08, 0x14 },   // *

    { 0x08, 0x08, 0x3E, 0x08, 0x08 },   // +

    { 0x00, 0x00, 0x50, 0x30, 0x00 },   // ,

    { 0x10, 0x10, 0x10, 0x10, 0x10 },   // -

    { 0x00, 0x60, 0x60, 0x00, 0x00 },   // .

    { 0x20, 0x10, 0x08, 0x04, 0x02 },   // /

    { 0x3E, 0x51, 0x49, 0x45, 0x3E },   // 0

    { 0x00, 0x42, 0x7F, 0x40, 0x00 },   // 1

    { 0x42, 0x61, 0x51, 0x49, 0x46 },   // 2

    { 0x21, 0x41, 0x45, 0x4B, 0x31 },   // 3

    { 0x18, 0x14, 0x12, 0x7F, 0x10 },   // 4

    { 0x27, 0x45, 0x45, 0x45, 0x39 },   // 5

    { 0x3C, 0x4A, 0x49, 0x49, 0x30 },   // 6

    { 0x01, 0x71, 0x09, 0x05, 0x03 },   // 7

    { 0x36, 0x49, 0x49, 0x49, 0x36 },   // 8

    { 0x06, 0x49, 0x49, 0x29, 0x1E },   // 9

    { 0x00, 0x36, 0x36, 0x00, 0x00 },   // :

    { 0x00, 0x56, 0x36, 0x00, 0x00 },   // ;

    { 0x08, 0x14, 0x22, 0x41, 0x00 },   // <

    { 0x14, 0x14, 0x14, 0x14, 0x14 },   // =

    { 0x00, 0x41, 0x22, 0x14, 0x08 },   // >

    { 0x02, 0x01, 0x51, 0x09, 0x06 },   // ?

    { 0x32, 0x49, 0x59, 0x51, 0x3E },   // @

    { 0x7E, 0x11, 0x11, 0x11, 0x7E },   // A

    { 0x7F, 0x49, 0x49, 0x49, 0x36 },   // B

    { 0x3E, 0x41, 0x41, 0x41, 0x22 },   // C

    { 0x7F, 0x41, 0x41, 0x22, 0x1C },   // D

    { 0x7F, 0x49, 0x49, 0x49, 0x41 },   // E

    { 0x7F, 0x09, 0x09, 0x09, 0x01 },   // F

    { 0x3E, 0x41, 0x49, 0x49, 0x7A },   // G

    { 0x7F, 0x08, 0x08, 0x08, 0x7F },   // H

    { 0x00, 0x41, 0x7F, 0x41, 0x00 },   // I

    { 0x20, 0x40, 0x41, 0x3F, 0x01 },   // J

    { 0x7F, 0x08, 0x14, 0x22, 0x41 },   // K

    { 0x7F, 0x40, 0x40, 0x40, 0x40 },   // L

    { 0x7F, 0x02, 0x0C, 0x02, 0x7F },   // M

    { 0x7F, 0x04, 0x08, 0x10, 0x7F },   // N

    { 0x3E, 0x41, 0x41, 0x41, 0x3E },   // O

    { 0x7F, 0x09, 0x09, 0x09, 0x06 },   // P

    { 0x3E, 0x41, 0x51, 0x21, 0x5E },   // Q

    { 0x7F, 0x09, 0x19, 0x29, 0x46 },   // R

    { 0x46, 0x49, 0x49, 0x49, 0x31 },   // S

    { 0x01, 0x01, 0x7F, 0x01, 0x01 },   // T

    { 0x3F, 0x40, 0x40, 0x40, 0x3F },   // U

    { 0x1F, 0x20, 0x40, 0x20, 0x1F },   // V

    { 0x3F, 0x40, 0x38, 0x40, 0x3F },   // W

    { 0x63, 0x14, 0x08, 0x14, 0x63 },   // X

    { 0x07, 0x08, 0x70, 0x08, 0x07 },   // Y

    { 0x61, 0x51, 0x49, 0x45, 0x43 },   // Z

    { 0x00, 0x7F, 0x41, 0x41, 0x00 },   // [

    { 0x02, 0x04, 0x08, 0x10, 0x20 },   // backslash

    { 0x00, 0x41, 0x41, 0x7F, 0x00 },   // ]

    { 0x04, 0x02, 0x01, 0x02, 0x04 },   // ^

    { 0x40, 0x40, 0x40, 0x40, 0x40 },   // _

    { 0x00, 0x01, 0x02, 0x04, 0x00 },   // '

    { 0x20, 0x54, 0x54, 0x54, 0x78 },   // a

    { 0x7F, 0x48, 0x44, 0x44, 0x38 },   // b

    { 0x38, 0x44, 0x44, 0x44, 0x20 },   // c

    { 0x38, 0x44, 0x44, 0x48, 0x7F },   // d

    { 0x38, 0x54, 0x54, 0x54, 0x18 },   // e

    { 0x08, 0x7E, 0x09, 0x01, 0x02 },   // f

    { 0x0C, 0x52, 0x52, 0x52, 0x3E },   // g

    { 0x7F, 0x08, 0x04, 0x04, 0x78 },   // h

    { 0x00, 0x44, 0x7D, 0x40, 0x00 },   // i

    { 0x20, 0x40, 0x44, 0x3D, 0x00 },   // j

    { 0x7F, 0x10, 0x28, 0x44, 0x00 },   // k

    { 0x00, 0x41, 0x7F, 0x40, 0x00 },   // l

    { 0x7C, 0x04, 0x18, 0x04, 0x78 },   // m

    { 0x7C, 0x08, 0x04, 0x04, 0x78 },   // n

    { 0x38, 0x44, 0x44, 0x44, 0x38 },   // o

    { 0x7C, 0x14, 0x14, 0x14, 0x08 },   // p

    { 0x08, 0x14, 0x14, 0x18, 0x7C },   // q

    { 0x7C, 0x08, 0x04, 0x04, 0x08 },   // r

    { 0x48, 0x54, 0x54, 0x54, 0x20 },   // s

    { 0x04, 0x3F, 0x44, 0x40, 0x20 },   // t

    { 0x3C, 0x40, 0x40, 0x20, 0x7C },   // u

    { 0x1C, 0x20, 0x40, 0x20, 0x1C },   // v

    { 0x3C, 0x40, 0x30, 0x40, 0x3C },   // w

    { 0x44, 0x28, 0x10, 0x28, 0x44 },   // x

    { 0x0C, 0x50, 0x50, 0x50, 0x3C },   // y

    { 0x44, 0x64, 0x54, 0x4C, 0x44 },    // z

    { 0x00, 0x08, 0x36, 0x41, 0x41 },   // {

    { 0x00, 0x00, 0x7F, 0x00, 0x00 },   // |

    { 0x41, 0x41, 0x36, 0x08, 0x00 },   // }

    { 0x02, 0x01, 0x01, 0x02, 0x01 },   // ~

    { 0x55, 0x2A, 0x55, 0x2A, 0x55 },   // del

};

/**

 * @defgroup lcd LCD

 * @brief Functions for the LCD

 * Functions for writing to the LCD.

 * All functions may be found in lcd.h.

 *

 * @{

 **/

/**

 * Initializes the LCD. Must be called before any other

 * LCD functions.

 **/

void lcd_init(void)

{

	LCDDDR |= (SCE | SDI | SCK);

	LCDRESETDDR |= (RST|D_C);

	LCDPORT &= ~( SCE | SDI | SCK);

	LCDRESETPORT &=~(D_C);

	SPCR |= 0x50;//0b01010000; // no SPI int, SPI en, Master, sample on rising edge, fosc/2

	SPSR |= 0x01;       // a continuation of the above

	LCDRESETPORT |= RST;

	delay_ms(10);

	LCDRESETPORT &= (~RST);

	delay_ms(100);

	LCDRESETPORT |= RST;

	lcd_putbyte( 0x21 );  // LCD Extended Commands.

	lcd_putbyte( 0xC8 );  // Set LCD Vop (Contrast).

	lcd_putbyte( 0x06 );  // Set Temp coefficent.

	lcd_putbyte( 0x13 );  // LCD bias mode 1:48.

	lcd_putbyte( 0x20 );  // LCD Standard Commands, Horizontal addressing mode.

	lcd_putbyte( 0x0C );  // LCD in normal mode.

	LCDRESETPORT |= D_C;		//put it in init instead of main

	lcd_clear_screen();

}

/**

 * Clears the LCD screen. lcd_init must be called first.

 * 

 * @see lcd_init

 **/

void lcd_clear_screen( void ) {

	int i;

	for (i = 0; i < 504; i++)

		lcd_putbyte(0x0);

	lcd_gotoxy(0,0);

}

/**

 * Prints a character on the LCD screen. lcd_init

 * must be called before this function may be used.

 *

 * @param c the character to print

 *

 * @see lcd_init

 **/

void lcd_putc(char c)

{

	int i;

	for (i = 0; i < 5; i++)

		lcd_putbyte(FontLookup[c-32][i]);

	lcd_putbyte(0);

}

/*

print an entire string to the lcd

*/

void lcd_puts(char *s)

{

	char *t = s;

	while (*t != 0)

	{

		lcd_putc(*t);

		t++;

	}

}

/*

go to coordinate x, y

y: vertically - 1 char

x: horizontally - 1 pixel

multiply x by 6 if want to move 1 entire character

origin (0,0) is at top left corner of lcd screen

*/

/**

 * Move the current cursor position to the one specified.

 * lcd_init must be called before this function may be used.

 * 

 * @param x The x coordinate of the new position

 * @param y The y coordinate of the new position

 *

 * @see lcd_init

 **/

void lcd_gotoxy(int x, int y)

{

	LCDRESETPORT &= ~(D_C);

	lcd_putbyte(0x40 | (y & 0x07));

	lcd_putbyte(0x80 | (x & 0x7f));

	LCDRESETPORT |= D_C;

}

/*

prints an int to the lcd

code adapted from Chris Efstathiou's code (hendrix@otenet.gr)

*/

/**

 * Print an integer to the LCD screen.

 * lcd_init must be called before this function may be used.

 * 

 * @param value the integer to print

 *

 * @see lcd_init

 **/

void lcd_puti(int value )

{

	unsigned char lcd_data[6]={'0','0','0','0','0','0' }, position=sizeof(lcd_data), radix=10; 

        /* convert int to ascii  */ 

        if(value<0) { 

	  lcd_putc('-'); 

	  value=-value; 

	}    

        do { 

	  position--; 

	  *(lcd_data+position)=(value%radix)+'0'; 

	  value/=radix;  

	} while(value); 

        /* start displaying the number */

        for( ; position<=(sizeof(lcd_data)-1); position++)

		lcd_putc(lcd_data[position]);

	return;

}

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

void lcd_putbyte(unsigned char b)

{

	SPDR = b;

	while (!(SPSR & 0x80)); /* Wait until SPI transaction is complete */

}

/**

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

#include "bom.h"

#include "dio.h"

#include "analog.h"

//constants

//TODO: for better readability, make all of these hex or decimal

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

// internal function prototypes

//TODO: if these are only used in this file, make them static

void output_high(int which);

void output_low(int 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 200

/**

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

 *

 * @{

 **/

/**

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

	int max_bom_temp = 0;

	//TODO: renamed a, i, and h to give them meaningful names

	int a, i, j, h;

    h = 255;

	//Turn off the loop so that we can actually use analog8 correctly

	analog_stop_loop();

	//Iterate through through each LED

    for (j = 0; j < 16; j++)

    {

      i = lookup[j];

      if (i&8)

        output_high(MONK3);

      else

        output_low(MONK3);

      if (i&4)

        output_high(MONK2);

      else

        output_low(MONK2);

      if (i&2)

        output_high(MONK1);

      else

        output_low(MONK1);

      if (i&1)

        output_high(MONK0);

      else

        output_low(MONK0);

      a = analog_get8(MONKI);

      if (a < h)

      {

        h = a;

        max_bom_temp = j;

      }

    }

	//Restart loop now that we are done using analog8

	analog_start_loop();

	//threshold on the bom analog value.

	//defined in bom.h

	// if the analog value read is above the threshold, we cannot see a robot

	// (remember, low means higher intensity).

	if(h < BOM_VALUE_THRESHOLD) {

		return max_bom_temp;

	}

	else

		return -1;

}

/**

 * Flashes the BOM. analog_init must be called before this

 * function can be used.

 * 

 * @see bom_off, analog_init

 **/

void bom_on(void)

{

  output_high(MONKL);

}

/**

 * Stops flashing the BOM. analog_init must be called

 * before this function can be used.

 * 

 * @see bom_on, analog_init

 **/

void bom_off(void)

{

  output_low(MONKL);

}

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

void output_high(int which) {

	digital_output(which, 1);

}

void output_low(int which) {

	digital_output(which, 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 analog.c

 * @brief Analog input and output

 *

 * Contains functions for manipulating the ADC on the Dragonfly board.

 * 

 * @author Colony Project, CMU Robotics Club

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

 **/

#include <util/delay.h>

#include <avr/interrupt.h>

#include "analog.h"

#include "serial.h"

// Internal Function Prototypes

//TODO: if this function is only used in this file, make it static

void set_adc_mux(int which);

/**

 * @defgroup analog Analog

 * Functions for manipulation the ADC on the dragonfly board.

 * All definitions may be found in analog.h.

 *

 * @{

 **/

//TODO: if these variables are only used in this file, make them static

int adc_loop_running = 0;

int adc_current_port = 0;

adc_t an_val[11];

/**

 * Initializes the ADC.

 * Call analog_init before reading from the analog ports.

 *

 * @see analog8, analog10

 **/

void analog_init(int start_conversion) {

	for (int i = 0; i < 11; i++) {

		an_val[i].adc10 = 0;

		an_val[i].adc8 = 0;

	}

	//cli();

	// ADMUX register

	// Bit 7,6 - Set voltage reference to AVcc (0b01)

	// Bit 5 - ADLAR set to simplify moving from register

	// Bit 4 - X

	// Bit 3:0 - Sets the current channel

	// Initializes to read from AN1 first (AN0 is reservered for the BOM)

	ADMUX = 0;

	ADMUX |= ADMUX_OPT | _BV(MUX0);

	// ADC Status Register A

	// Bit 7 - ADEN is set (enables analog)

	// Bit 6 - Start conversion bit is set (must be done once for free-running mode)

	// Bit 5 - Enable Auto Trigger (for free running mode) NOT DOING THIS RIGHT NOW

	// Bit 4 - ADC interrupt flag, 0

	// Bit 3 - Enable ADC Interrupt (required to run free-running mode)

	// Bits 2-0 - Set to create a clock divisor of 128, to make ADC clock = 8,000,000/64 = 125kHz

	ADCSRA = 0;

	ADCSRA |= _BV(ADEN) | _BV(ADIE) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0);

	// Set external mux lines to outputs

	DDRG |= 0x1C;

	// Set up first port for conversions

	set_adc_mux(0x00);

	adc_current_port = AN1;

	//Start the conversion if requested

	if (start_conversion)

		analog_start_loop();

	else

		analog_stop_loop();

	//sei();

}	

unsigned int analog8(int which) {

	if (which == BOM_PORT) {

		return 0;

	} else {

		return an_val[which - 1].adc8;

	}

}

unsigned int analog10(int which) {

	if (which == BOM_PORT) {

		return 0;

	} else {

		return an_val[which - 1].adc10;

	}

}

void analog_start_loop(void) {

	//Start the conversion

	ADCSRA |= _BV(ADSC);

	adc_loop_running = 0x1;

}

//will stop after current conversion finishes

void analog_stop_loop(void) {

	//Stop the conversion

	adc_loop_running = 0x0;

}

/**

 * Reads an eight bit number from an analog port.

 * analog_init must be called before using this function.

 * 

 * @param which the analog port to read from. One of

 * the constants AN0 - AN7.

 *

 * @return the eight bit input to the specified port

 *

 * @see analog_init, analog10

 **/

unsigned int analog_get8(int which)

{	

	// Let any previous conversion finish

	while (ADCSRA & _BV(ADSC));

	if(which < EXT_MUX) {

		ADMUX = ADMUX_OPT + which;

	} else {

		ADMUX = ADMUX_OPT + EXT_MUX;

		set_adc_mux(which - 8);

	}

	// Start the conversion

	ADCSRA |= _BV(ADSC);

	// Wait for the conversion to finish

	while (ADCSRA & _BV(ADSC));

	return ADCH; //since we left aligned the data, ADCH is the 8 MSB.

}

/**

 * Reads a ten bit number from the specified port.

 * analog_init must be called before using this function.

 * 

 *

 * @param which the analog port to read from. Typically

 * a constant, one of AN0 - AN7.

 *

 * @return the ten bit number input to the specified port

 * 

 * @see analog_init, analog8

 **/

unsigned int analog_get10(int which)

{

	int adc_h;

	int adc_l;

	// Let any previous conversion finish

	while (ADCSRA & _BV(ADSC));

	if(which < EXT_MUX) {

		ADMUX = ADMUX_OPT + which;

	} else {

		ADMUX = ADMUX_OPT + EXT_MUX;

		set_adc_mux(which - 8);

	}

	// Start the conversion

	ADCSRA |= _BV(ADSC);

	// Wait for the conversion to finish

	while (ADCSRA & _BV(ADSC));

	adc_l = ADCL;

	adc_h = ADCH;

	return ((adc_h << 2) | (adc_l >> 6));

}

/**

 * Returns the current position of the wheel, as an integer

 * in the range 0 - 255.

 * analog_init must be called before using this function.

 *

 * @return the orientation of the wheel, as an integer in

 * the range 0 - 255.

 *

 * @see analog_init

 **/

int wheel(void)

{

	return analog8(WHEEL_PORT);

}

/**

 * Sets the value of the external analog mux. Values are read

 * 	on AN7 physical port. (AN8 - AN15 are "virtual" ports).

 *

 * @param which which analog mux port (0-7) which corresponds

 * 		  to AN8-AN15.

 *

 * @bug FIX THIS IN THE NEXT BOARD REVISION:

 *		ADDR2 ADDR1 ADDR0

 *		G2.G4.G3 set mux to port 0-7 via vinary selection

 *		math would be much cleaner if it was G4.G3.G2

 *

 * @see analog_init

 **/

void set_adc_mux(int which)

{  

  // mask so only proper bits are possible.  

  PORTG = (PORTG & 0xE3) | ((which & 0x03) << 3) | (which & 0x04);

}

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

ISR(ADC_vect) {

	static volatile int adc_prev_loop_running = 0;

	int adc_h = 0;

	int adc_l = 0;

	//usb_putc('p');

	//usb_puti(adc_current_port);

	//usb_putc('r');

	//usb_puti(adc_loop_running);

	//usb_puts("\n\r");

	//Store the value only if this read isn't for the BOM

	if (ADMUX != BOM_PORT) {

		adc_l = ADCL;

		adc_h = ADCH;

		an_val[adc_current_port - 1].adc10 = (adc_h << 2) | (adc_l >> 6);

		an_val[adc_current_port - 1].adc8 = adc_h;

		//usb_puti(an_val[adc_current_port - 1].adc10);

		//usb_puts("\n\r");

		//usb_puti(an_val[adc_current_port - 1].adc8);

		//usb_puti(ADCH);

		//usb_puts("\n\r");

	}

	//Save the result only if we just turned off the loop

	if (!adc_loop_running && !adc_prev_loop_running)

		return;

	adc_prev_loop_running = adc_loop_running;

	//Skip AN7 because it is not a real port

	if (adc_current_port == AN6) {

		ADMUX = ADMUX_OPT | EXT_MUX;

		set_adc_mux(AN8 - 8);

		adc_current_port = AN8;

	//Wrap around

	} else if (adc_current_port == AN11) {

		adc_current_port = AN1;

		ADMUX = ADMUX_OPT | adc_current_port;

	//Normal increment

	} else {

		adc_current_port++;

		if(adc_current_port < EXT_MUX) {

			ADMUX = ADMUX_OPT | adc_current_port;

		} else {

			ADMUX = ADMUX_OPT | EXT_MUX;

			set_adc_mux(adc_current_port - 8);

		}

	}

	//Initiate next conversion only if we are running a loop

	if (!adc_loop_running)

		return;

	ADCSRA |= _BV(ADSC);

	//if (ADCSRA & _BV(ADSC))

	//	usb_putc('s');

	return;

}

/**

 * 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.c

 * @brief Dragonfly initialization

 *

 * Contains implementation of dragonfly_init.

 *

 * @author Colony Project, CMU Robotics Club

 **/

#include <dragonfly_lib.h>

/* init_dragonfly - Initializes functions based on configuration parameters

   examples:

   init_dragonfly (0, 0, 0); - just initialize the digital IO (buttons, potentiometer)

   init_dragonfly (ANALOG | SERIAL | BUZZER, C0_C1_ANALOG, BAUD115200); 

   Initialize ADC and set C0 and C1 as analog inputs.  Initialize serial and set baud rate

   to 115200 bps.  Initialize the buzzer.

   init_dragonfly (MOTORS | ORB, 0, 0);

   Initialize motor driving and the color fading abilities of the ORB. */

/**

 * @defgroup dragonfly Dragonfly

 * @brief General Dragonfly Functions

 * General functions for the dragonfly. Include

 * dragonfly_lib.h to access these functions.

 *

 * @{

 **/

/**

 * Initializes the components specified by config.

 * 

 * @see analog_init, usb_init, xbee_init, buzzer_init,

 * bom_init, orb_init, motors_init, lcd_init

 **/

void dragonfly_init(int config) 

{

		sei();

	// Set directionality of various IO pins

	DDRG &= ~(_BV(PING0)|_BV(PING1));

	PORTG |= _BV(PING0)|_BV(PING1);

	if(config & ANALOG)

		analog_init(ADC_START);

	if(config & COMM)

	{

		//Defaults to 115200. Check serial.h for more information.

		usb_init();

		xbee_init();

	}

	if(config & BUZZER)

	{

		sei();

		buzzer_init();

	}

	if(config & ORB)

	{

		sei();

		orb_init();

	}

	if(config & MOTORS)

		motors_init();

	if(config & LCD)

		lcd_init();

	if(config & ORB)

	{

		sei();

		orb_init();

	}

	// delay a bit for stability

	_delay_ms(1);

}

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

#include "rangefinder.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.

*/

#define MIN_IR_ADC8 20

#define MAX_IR_ADC8 133

static int IR_dist_conversion[114] = {

800,791,751,714,681,651,623,597,574,552,531,512,494,478,462,447

,434,421,408,397,386,375,365,356,347,338,330,322,315,307,301,294

,288,282,276,270,265,260,255,250,245,241,237,232,228,224,221,217

,213,210,207,203,200,197,194,191,189,186,183,181,178,176,173,171

,169,166,164,162,160,158,156,154,152,151,149,147,145,144,142,140

,139,137,136,134,133,131,130,129,127,126,125,124,122,121,120,119

,118,117,115,114,113,112,111,110,109,108,107,106,105,105,104,103

,102,101

};

int linearize_distance(int value);

/**

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