Colony

/**

 * Eugene Marinelli

 * 1/31/07

 *

 * robot_dongle_test - trying to figure out how to get bytes from the robot.

 */

#include <unistd.h>

#include <stdio.h>

#include <fcntl.h>

int main()

{

  int fd = open("/dev/ttyS0", O_RDWR);

  char buf[80];

  while(1){

    //write(fd, "0", 1);

    if(read(fd, buf, 1)){

      printf("%c\n", buf[0]);

    }

  }

  close(fd);

  return 0;

}

/*

Wireless

Eugene Marinelli

(See .h for details)

*/

/* Includes */

#include <firefly+_lib.h>

#include <string.h>

#include <avr/interrupt.h>

#include "wireless.h"

/* Constants */

#define PREFIX0 'R'

#define PREFIX1 'C'

//wl_buf positions

#define WL_SRC_LOC 2

#define WL_DEST_LOC 3

#define WL_DATA_START 4

#define WL_TIMEOUT 4

#define WL_TIMEOUT_INITIAL 4

/* Globals */

static char wl_buf[WL_PACKET_MAX_LEN];

static WL_Packet latest_packet;

static WL_Packet tmp_packet;

static char new_message;

static char listener_addr;

static int message_length;

static unsigned int wl_index;

static unsigned char wl_chksum;

static unsigned char wl_to_flag;

static unsigned int wl_to_count;

static unsigned int wl_to_max;

static char first_time;

/* Internal function headers */

static void wl_timeout_handler(void);

/* External function definitions */

int wl_init(int msg_len, char listener_address){

  if(msg_len > WL_MSG_MAX_LEN){

    printf("Error: %s: message length must be less than %d\n", __FUNCTION__, 

	   WL_MSG_MAX_LEN);

    return -1;

  }

  message_length = msg_len;

  listener_addr = listener_address;

  wl_index = 0;

  wl_chksum = 1;

  rtc_init(PRESCALE_DIV_128, 64, &wl_timeout_handler); 

  /* executes the function about every 1/4 sec */

  wl_to_flag = 0;

  wl_to_count = 0; 

  first_time = 1;

  UCSR0B |= _BV(RXCIE) | _BV(RXEN);

  sei();

  new_message = 0;

  memset(&latest_packet, 0, sizeof(WL_Packet));

  return 0;

}

int wl_send(char* msg, char dest){

  int i = 0;

  WL_Packet packet;

  if(strlen(msg) > WL_MSG_MAX_LEN){

    printf("Error: %s: message is too long\n", __FUNCTION__);

    return -1;

  }

  wl_create_packet(msg, listener_addr, dest, &packet);

  serial_putchar(packet.prefix[0]);

  serial_putchar(packet.prefix[1]);

  serial_putchar(packet.src);

  serial_putchar(packet.dest);

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

    serial_putchar(packet.msg[i]);

  }

  serial_putchar(packet.checksum);

  //printf("%x %x %x %x", packet.prefix[0], packet.prefix[1], packet.src, 

  //	 packet.dest);

  return 0;

}

int wl_recv(char* msgbuf, char* src, char* dest){

  if(!new_message){

    return 0; //no new message

  }

  strcpy(msgbuf, latest_packet.msg);

  *src = latest_packet.src;

  *dest = latest_packet.dest;

  new_message = 0;

  return strlen(msgbuf);

}

char wl_get_checksum(WL_Packet* packet){

  char checksum = 0;

  int i;

  checksum += packet->src;

  checksum += packet->dest;

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

    checksum += packet->msg[i];

  }

  return checksum;

}

int wl_create_packet(char* msg, char src, char dest, WL_Packet* packet){

  packet->prefix[0] = 'R';

  packet->prefix[1] = 'C';

  packet->src = src;

  packet->dest = dest;

  memset(packet->msg, 0, message_length);

  strcpy(packet->msg, msg);

  packet->checksum = wl_get_checksum(packet);

  return 0;

}

/* Internal function definitions */

//use on new avr-libc

//ISR(USART0_RX_vect)

SIGNAL(SIG_USART0_RECV){

  char wl_input = UDR0;

  /* LOCAL HACK!!! */

  //orb_set_color(ORANGE);

  //printf("%x ", wl_input);

  lcd_putint(wl_input);

  if(wl_index == 0){

    //first packet? - check chksum to make sure we're not beginning

    if(wl_input == PREFIX0) {

      wl_to_count = 0; //at this point we're not timing out

      wl_index = 1;

    }

  }else if(wl_index == 1){

    if(wl_input != PREFIX1){

      wl_index = 0;

    }else{

      wl_index = WL_SRC_LOC;

    }

  }else if(wl_index == WL_SRC_LOC){

    tmp_packet.src = wl_input;

    wl_index = WL_DEST_LOC;

  }else if(wl_index == WL_DEST_LOC){

    tmp_packet.dest = wl_input;

    wl_index = WL_DATA_START;

  }else if(wl_index < WL_DATA_START + message_length){

    //getting the rest of the packet

    tmp_packet.msg[wl_index-WL_DATA_START] = wl_input;

    wl_index++;

    //printf("%d: %d; %d\n", wl_index, wl_input, wl_chksum);

  }else if(wl_index == WL_DATA_START + message_length){

    //we are at checksum location

    tmp_packet.checksum = wl_input;

    wl_chksum = wl_get_checksum(&tmp_packet);

    if(tmp_packet.checksum == wl_chksum) {

      //checksum is correct

      memcpy(&latest_packet, &tmp_packet, sizeof(WL_Packet));

      new_message = 1;

    } else { //chksum failed

      serial1_putchar('F');

      lcd_putchar('F');

      printf("checksum failed - was %d, should have been %d\n", wl_input, 

	     wl_chksum);

    }

    wl_index = 0;

  }

}

/*

  \fn wl_timeout_handler

  Handles the timeout case.

  A time out occurs about every 1/4 second.  This function is called each time one occurs.

  In the case where a robot first turns on, it will wait for a long time before declaring itself the leader.

  Otherwise, if enough time-outs occur in a row, wl_to_flag will be set to 1.  This will cause a timeout packet

  to be sent next time around

*/

static void wl_timeout_handler( void ) {

  //increment the total number of wireless time-outs

  wl_to_count++;

  static int wl_first_to_count =  0;

  //first time time out routine - listen for a long time

  if(first_time) {

    if(wl_to_count > WL_TIMEOUT_INITIAL) {

      //increment the counter for first timeouts

      wl_first_to_count++;

      //you don't actually want to send out a packet 

      //-- you would confuse other robots

      wl_to_flag = 0;

      wl_to_count = 0;

    }

  } else {

    //not the first time - regular time out routine

    //if you have enough timeouts

    if(wl_to_count > WL_TIMEOUT) {

      //lcd_putchar('z');

      //change the time-out flag to 1

      wl_to_flag = 1;

      wl_to_count = 0;

    }

  }

}

#include "serial.h"

char helper(char c){

	switch(c){

		case 0x00: return '0';

		case 0x01: return '1';

		case 0x02: return '2';

		case 0x03: return '3';

		case 0x04: return '4';

		case 0x05: return '5';

		case 0x06: return '6';

		case 0x07: return '7';

		case 0x08: return '8';

		case 0x09: return '9';

		case 0x0A: return 'A';

		case 0x0B: return 'B';

		case 0x0C: return 'C';

		case 0x0D: return 'D';

		case 0x0E: return 'E';

		case 0x0F: return 'F';

		default: return '?';

	}

}

void print_hex(char val){

	char hgh = (val & 0xF0) >> 4;

	char low = (val & 0x0F);

	serial1_putchar(helper(hgh));

	serial1_putchar(helper(low));

}

/*  

	servo.c - Contains functions and interrupts necessary to activate servos

	author:  Tom Lauwers

	3/1/06 Iain

		Man, I don't know what a servo is. 

*/

#include <avr/io.h>

#include <avr/interrupt.h>

#include <servo.h>

#include <dio.h>

#include <lights.h>

#include <lcd.h>

//1677.7216 c/ms

//i dunno must be 2^k

//250

//9C40 ~ 20ms

//#A000 ~ 20.48 but prolly wont work

//div by 256: 6.55078125 c/ms 80 ~ 19.5ms

#define SERVO_PERIOD 0x80

//needs be 1ms

//is 1.22ms

#define SERVO_CONSTANT 0x8

/* dirty dirty

unsigned int servo_vals[4] = {0,0,0,0}; // Stores the set servo values

int current_servo = 0;  // Stores which servo is current in the interrupt routine

*/

unsigned int servo_vals[8] = {0,0,0,0,0,0,0,0}; // Stores the set servo values

int phase_time=0;

int phase_base=0;

int servos_on = 0;      // Stores which servos are enabled

int servos_enabled=0; //which are high

int servo_flag = 0;     // Stores whether or not servos are enabled

/* Timer 1 output compare B interrupt.  Does the following:

	Checks to see if the current servo is 4, in which case just need

	to set low servo 3's signal and return.

	Else, check to see if the current servo is enabled, and set it's signal high

	Then, set the previous servo's signal low

	Now, set the next output compare to occur for some time from now 

	specified by the current servo's value

*/

SIGNAL (SIG_OUTPUT_COMPARE3B)

{

	lcd_putchar('i');

	if(servo_flag == 0)

		return;

	int i;

	if(phase_time == 0){

		lcd_putchar('p');

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

			if(servos_on & _BV(i))

				digital_output((_PORT_E << 3) +i,1);

		}

		servos_enabled = servos_on;

		phase_base = OCR3B;

	}

	unsigned int min=~0;

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

		if(_BV(i) & servos_enabled){

			if(SERVO_CONSTANT*servo_vals[i] <= phase_time){

				digital_output((_PORT_E << 3) + i,0);

				servos_enabled &= ~_BV(i);

			}

			else if(SERVO_CONSTANT*servo_vals[i] < min) {

				min = servo_vals[i];

			}

		}

	}

	if(!servos_enabled){

		OCR3B = phase_base + SERVO_PERIOD;

		phase_time = 0;

	}

	else {

		OCR3B = phase_base + min;

		phase_time = min;

	}

}	

void init_servo()

{

	//1010 1001

	TCCR3A = 0; 

	//0000 0100

	TCCR3B = 0x4;  //prescaler to 256

	TCCR3C=0;

	lcd_init();

	ETIMSK |= 0x08;

	servo_flag = 1;

	OCR3B = 0x0;

	lcd_putchar('i');

}

// Enable a servo specified by config

void enable_servo(int config)

{

	servos_on |= _BV(config);

}

// Disable a servo specified by config

void disable_servo(int config)

{

	servos_on &= (0xFF-_BV(config));

}

// Disables the timer1 interrupt, disabling the servos

void disable_servos()

{

	// Disables interrupts

	ETIMSK &= ~0x08;

	servo_flag=0;

}

// Enables the timer1 interrupt, enabling the servos

void enable_servos()

{

	// Enable interrupts on output compare B

	ETIMSK |= 0x08;

	servo_flag=1;

}

// Set a servo to a value.  Automatically enables the servo.

void set_servo(int servo, unsigned int value)

{

	enable_servo(servo);

	servo_vals[servo] = value;

}

/*

lights.h

most of this is shamelessly copied from FWR's orb.h (Tom Lauwers and Steven Shamlian)

author: CMU Robotics Club, Colony Project

*/

#ifndef _LIGHTS_H_

#define _LIGHTS_H_

//user LED

#ifdef FFPP

#define USERLED 15

#define USERLED1 15

#define USERLED2 31

#else

#define USERLED PING2

#define USERLED1 PING2

#define USERLED2 PING2 //only have 1

//LEDs are on bank E

#define REDLED   PE3

#define GREENLED PE4

#define BLUELED  PE5

#endif

//ORB

#define RED       0xE0

#define ORANGE    0xE8

#define YELLOW    0xFC

#define LIME      0x7C

#define GREEN     0x1C

#define CYAN      0x1F

#define BLUE      0x03

#define PINK      0x63

#define PURPLE    0x23

#define MAGENTA   0xE3

#define WHITE     0xFF

#define ORB_OFF   0x00

//?

#define COUNT_START 0x8000

//user LED

void led_init( void );

void led_user(int value);

// For function descriptions see orb.c 

void orb_init(void);

void orb_set(unsigned int red_led, unsigned int green_led, unsigned int blue_led); 

void orb_set_color(int col);

void orb_disable(void);

void orb_enable(void);

void orb_set_dio(int red, int green, int blue);

#ifdef FFPP

int orbs[32];

//send now

void orb_set_num(unsigned char num, unsigned int red_led, unsigned int green_led, unsigned int blue_led);

//no send

void orb_set_num_ns(unsigned char num, unsigned int red_led, unsigned int green_led, unsigned int blue_led);

//force send

void orb_send(void);

void tlc5940test(void);

void tlc_clock1(int data);//may not need these 2

void tlc_clock2(int data);

void tlc_send(void);

void tlc_latch(void);

void tlc5940init(void);

#endif

#endif

#ifndef WIRELESS_H

#define WIRELESS_H

/*

Wireless - wireless functions for Colony

Eugene Marinelli

7/22/06

*/

#define WL_MSG_MAX_LEN 16

#define WL_PACKET_MAX_LEN (WL_MSG_MAX_LEN+5)

#define GLOBAL_DEST 200

typedef struct {

  char prefix[2];

  char src;

  char dest;

  char msg[WL_MSG_MAX_LEN];

  char checksum;

} WL_Packet;

int wl_init(int msg_len, char listener_address);

int wl_send(char* msg, char dest);

int wl_recv(char* msgbuf, char* src, char* dest);

// get most recent valid message - implement message queue later

int wl_create_packet(char* msg, char src, char dest, WL_Packet* packet);

char wl_get_checksum(WL_Packet* packet);

#endif

/*

lcd-ar2.c - implementation for lcd functions

Author: CMU Robotics Club, Colony Project

This uses SPI.

lcd_init MUST be called before anything can be used.

*/

#include <avr/io.h>

#include <lcd.h>

#include <time.h>

#include "oled.h"

/*

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

};

/*

initializes the LCD

*/

void lcd_init(void)

{

#ifdef FFPP

	OLED_init();

#else

	LCDDDR |= (D_C | SCE | SDI | SCK);

	LCDRESETDDR |= RST;

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

	SPCR |= 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.

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

	lcd_clear_screen();

#endif

}

/*

clear the lcd screen

*/

void lcd_clear_screen( void ) {

#ifdef FFPP

	drawclear();

#else

	int i;

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

			lcd_putbyte(0x0);

	lcd_gotoxy(0,0);

#endif

}

/*

print a byte on the lcd screen

*/

void lcd_putbyte(unsigned char b)

{

#ifndef FFPP

	SPDR = b;

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

#endif

}

/*

print a character on the lcd

*/

void lcd_putchar(char c)

{

#ifndef FFPP

	int i;

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

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

	lcd_putbyte(0);

#endif

}

/*

print an entire string to the lcd

*/

void lcd_putstr(char *s)

{

	char *t = s;

	while (*t != 0)

	{

		lcd_putchar(*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

*/

void lcd_gotoxy(int x, int y)

{

#ifndef FFPP

  LCDPORT &= ~(D_C);

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

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

  LCDPORT |= D_C;

#endif

}

/*

prints an int to the lcd

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

*/

void lcd_putint(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_putchar('-'); 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_putchar(lcd_data[position]);

          }

	return;

}

#include "firefly+_lib.h"

#include "follow.h"

#define FWD 0

#define BCK 1    

#define TOTAL 16

/*

int find_max

returns the index of the BOM with the highest reading

* note: index, not value

*/

void seek(int relative_position) 

{

	int speed = 255;

	//MOTOR1 - left

	//MOTOR2 - right

	if(relative_position < 5 || relative_position > 13)

	{ //turn right

		motor2_set(BCK, 0);

		motor1_set(FWD, speed);

	}

	else if(relative_position > 5 && relative_position <= 13)

	{ //turn left

		motor2_set(FWD, speed);

		motor1_set(BCK, 0);

	}

	else 

	{ //go forward

		//turn_off_motors();

		motor1_set(FWD, speed);

		motor2_set(FWD, speed);

	}

}

void drive_forward (void)

{

    int speed = 255;

    motor1_set(FWD, speed);

    motor2_set(FWD, speed);

}

/*  

	servo.h - Contains function prototypes for servo activation

	author:  Tom Lauwers

*/

#ifndef _SERVO_H_

#define _SERVO_H_

// Information on functions available in servo.c

void init_servo(void);

void enable_servos(void);

void enable_servo(int config);

void set_servo(int servo, unsigned int value);

void disable_servos(void);

void disable_servo(int );

#endif

#ifndef _FOLLOW_H

#define _FOLLOW_H

void seek(int velocity);

void drive_forward (void);

#endif

#ifndef _LCD_H_

#define _LCD_H_

#ifndef FFPP

//////lcd defines

#define RST 0x04  // pe2 (GPIO)

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

#define D_C 0x10  // pb4 (GPIO?)

#define SDI 0x04  // pb2 (MOSI)

#define SCK 0x02  // pb1 (SCK)

#define LCDPORT PORTB

#define LCDDDR DDRB

#define LCDRESETPORT PORTE

#define LCDRESETDDR DDRE

#else

// other OLED pins

//#define LCD_RW    0x00000080

#define LCD_RS    PB4 // Command/Data

//#define LCD_RD    0x00008000

#define LCD_RSTB  PE2 // reset line

#define LCD_CS    PB0

// inialize OLED and SPI

void OLED_init(void);

// reset Controller

void Reset_SSD1339(void);

// write command or data

void write_c(unsigned char out_command);

void write_d(unsigned char out_data);

// these write data to the OLED on 8 bit data bus,  depends on MCU

void LCD_out(unsigned char cmd);

// these functions set / clear pins for OLED control lines.  they accecpt a 0 or 1 

//void RD(char stat);

//void RW(char stat);

void DC(char stat);

void RES(char stat);

void CS(char stat);

void drawcircle(char col, char row, char radius, int lcolor,  int fcolor);

void drawrect(char scol, char srow, char ecol, char erow, int lcolor,  int fcolor);

void drawline(char scol, char srow, char ecol, char erow, int lcolor);

void drawcopy(char scol, char srow, char ecol, char erow, char ncol, char nrow);

void drawfill(char fill);

void drawclearwindow(char scol, char srow, char ecol, char erow);

inline void drawclear(void);

void drawscroll(char hoff, char srow, char rows, char speed);

inline void drawscrollstart(void);

inline void drawscrollstop(void);

void drawIR(void);

void crazycircle (void);

void crazyrect (void) ;

void OLEDtest (void) ;

#endif

void lcd_init(void);

void lcd_clear_screen( void );

void lcd_putbyte(unsigned char c);

void lcd_putchar(char c);

void lcd_putstr(char *s);

void lcd_gotoxy(int x, int y);

void lcd_putint(int value);

#endif

/*

Eugene Marinelli 

7/22/06

*/

/* Includes */

#include <stdio.h>

#include <string.h>

#include <firefly+_lib.h>

#include "pindefs_ff.h"

#include <wireless.h>

void init_hardware(void);

/* Main */

int main(void){

  char buf[80];

  char src, dest;

  init_hardware();

  wl_init(15,1);

  orb_set_color(BLUE);

  while(1){

    if(wl_recv(buf, &src, &dest)){

      printf("%s", buf);

      orb_set_color(RED);

    }else{

      printf(".");

      orb_set_color(BLUE);

    }

    wl_send("ABC", 200); 

    delay_ms(500);

  }

  return 0;

}

void init_hardware(){

  motors_init();

  orb_init();

  led_init();

  analog_init();

  serial_init(BAUD9600);

  serial1_init(BAUD115200);

  lcd_init();

  fdevopen(&serial1_putchar, &serial1_getchar);

}

/* 

dio.c

Controls digital input and output

A general note on how this code works:

portpin is used to select both the bank and which pin is selected

6 bits are used (lower 6, ex: 0b00abcdef)

	the first 3 (abc in this example) are used to select the bank

		A = 001

		B = 010

		C = 011

		D = 100

		E = 101

		F = 110

		G = 111

		the bank can be found by doing portpin >> 3

	the next 3 (def in this example) are used to select the pin number

	the 3 bits are just the binary representation of the pin number

		0 = 000

		1 = 001

		2 = 010

		3 = 011

		4 = 100

		5 = 101

		6 = 110

		7 = 111

		the pin number can be found by doing portping & 0b111

*/

#include <avr/interrupt.h>

#include <dio.h>

#include <time.h>

#include <lights.h>

/*

digital_input

reads the value on the selected portpin, returns it as 1 or 0

see general description (above) for definition of portpin

*/

int digital_input(int portpin){

	int pin = portpin & 0x7;

	int pin_val = 0;

	switch(portpin >> 3){

		case _PORT_A:

			DDRA &= ~_BV(pin);

			pin_val = PINA;

			return (pin_val >> pin) & 1;

		case _PORT_B:

			DDRB &= ~_BV(pin);

			pin_val = PINB;

			return (pin_val >> pin) & 1;

		case _PORT_C:

			DDRC &= ~_BV(pin);

			pin_val = PINC;

			return (pin_val >> pin) & 1;

		case _PORT_D:

			DDRD &= ~_BV(pin);

			pin_val = PIND;

			return (pin_val >> pin) & 1;

		case _PORT_E:

			DDRE &= ~_BV(pin);

			pin_val = PINE;

			return (pin_val >> pin) & 1;

		case _PORT_F:

			if(pin>=4){

				MCUSR|=1<<7;

				MCUSR|=1<<7;

			}

			DDRF &= ~_BV(pin);

			pin_val = PINF;

			return (pin_val >> pin) & 1;

		case _PORT_G:

			DDRG &= ~_BV(pin);

			pin_val = PING;

			return (pin_val >> pin) & 1;

		default: break;

	}

	return -1;

}

/*

digital_pull_up

Enables pullup on a pin.  if pin is output, it will make it output 1.

*/

void digital_pull_up(int portpin) {

	int pins = portpin & 0x07;

	switch(portpin >> 3) {

		case _PORT_A:

			PORTA |= _BV(pins);

			break;

		case _PORT_B:

			PORTB |= _BV(pins);

			break;		

		case _PORT_C:

			PORTC |= _BV(pins);

			break;		

		case _PORT_D:

			PORTD |= _BV(pins);

			break;		

		case _PORT_E:

			PORTE |= _BV(pins);

			break;		

		case _PORT_F:

			PORTF |= _BV(pins);

			break;		

		case _PORT_G:

			PORTG |= _BV(pins);

			break;

	}

}

/*

digital_output

sets portpin to the value

see general description above for explanation of portpin

val can only be 0 for off, nonzero for on

*/

void digital_output(int portpin, int val) {

	int pins = portpin & 0x07;

	/*

	if you want to set to 0

	*/

	if(val == 0) {

		switch(portpin >> 3) {

			case _PORT_A:

				DDRA |= _BV(pins);

				PORTA &= (0XFF - _BV(pins));

				break;

			case _PORT_B:

				DDRB |= _BV(pins);

				PORTB &= (0XFF - _BV(pins));

				break;		

			case _PORT_C:

				DDRC |= _BV(pins);

				PORTC &= (0XFF - _BV(pins));

				break;		

			case _PORT_D:

				DDRD |= _BV(pins);

				PORTD &= (0XFF - _BV(pins));

				break;		

			case _PORT_E:

				DDRE |= _BV(pins);

				PORTE &= (0XFF - _BV(pins));

				break;		

			case _PORT_F:

				DDRF |= _BV(pins);

				PORTF &= (0XFF - _BV(pins));

				break;		

			case _PORT_G:

				DDRG |= _BV(pins);

				PORTG &= (0XFF - _BV(pins));

				break;

		}

	}

	else { /* ( val == 1) */ 

		switch(portpin >> 3) {

			case _PORT_A:

				DDRA |= _BV(pins);

				PORTA |= _BV(pins);

				break;

			case _PORT_B:

				DDRB |= _BV(pins);

				PORTB |= _BV(pins);

				break;		

			case _PORT_C:

				DDRC |= _BV(pins);

				PORTC |= _BV(pins);

				break;		

			case _PORT_D:

				DDRD |= _BV(pins);

				PORTD |= _BV(pins);

				break;		

			case _PORT_E:

				DDRE |= _BV(pins);

				PORTE |= _BV(pins);

				break;		

			case _PORT_F:

				DDRF |= _BV(pins);

				PORTF |= _BV(pins);

				break;		

			case _PORT_G:

				DDRG |= _BV(pins);

				PORTG |= _BV(pins);

				break;

		}

	}

}

//////////////////////////////////////

////////////   button1  //////////////

//////////////////////////////////////

/*

return 1 if button is pressed, 0 otherwise

*/

int button1_read( void )

{

	//return (BTN & (_BV(BTN1)) >> BTN1);

	return (PIN_BTN >> BTN1) & 1;

}

/*

similar to button1_read, but hold program until the button is actually pressed

*/

void button1_wait( void )

{

	while(!button1_read() ) {

		delay_ms(15);

	}

}

/*

same as button1_wait

However, blink the led while waiting

IMPORTANT: This requires that the LED has been initialized (  init_led  )

*/

void button1_wait_led( void )

{

	int i = 0;

	while(!button1_read() ) {

		if( i < 8 )

			led_user(1);

		else {

			led_user(0);

		}

		//increment i, but restart when i = 15;

		i = (i+1) & 0xF;

		delay_ms(15);

	}

	led_user(0);

}

//////////////////////////////////////

////////////   button2  //////////////

//////////////////////////////////////

//see button1 functions for descriptions

//same except for which button is used

int button2_read( void )

{

	return (PIN_BTN >> BTN2) & 1;

}

void button2_wait( void )

{

	while(!button2_read() ) {

		delay_ms(15);

	}

}

void button2_wait_led( void )

{

	int i = 0;

	while(!button2_read() ) {

		if( i < 8 )

			led_user(1);

		else {

			led_user(0);

		}

		//increment i, but restart when i = 15;

		i = (i+1) & 0xF;

		delay_ms(15);

	}

	led_user(0);

}

/*

// EXTERNAL INTERRUPTS

/// example code to be used by anyone in need

/// this example has 2 bump sensors on PE6 and PE7

// left touch sensor on PE6 

SIGNAL (SIG_INTERRUPT6)

{

	putcharlcd('6');

}

// right touch sensor on PE7

SIGNAL (SIG_INTERRUPT7)

{

	putcharlcd('7');

}

*/

/*

	serial.c - Functions for using the RS232 serial port

	author: Robotics Club, Colony Project

	much code taken from FWR's library, author: Tom Lauwers

	general note - serial -> uart

				   serial1 -> uart1

		this is done for clarity purposes

*/

#include <avr/io.h>

#include <stdio.h>

#include "serial.h"

//setup uart0 (serial)

void serial_init(unsigned int ubrr)

{

	/*Set baud rate - baud rates under 4800bps unsupported */

	UBRR0H = 0x00;

	UBRR0L = (unsigned char)ubrr;

	/*Enable receiver and transmitter */

	UCSR0B |= (1<<RXEN0)|(1<<TXEN0);

	/* Set frame format: 8data, 1stop bit, asynchronous normal mode */

	UCSR0C |= (1<<UCSZ00) | (1<<UCSZ01);

}

// uart_putchar - Low-level function which puts a value into the 

// Tx buffer for transmission.  Automatically injects

// a \r before all \n's

int serial_putchar(char c)

{

    if (c == '\n')

		putchar('\r');

	// Wait until buffer is clear for sending

    loop_until_bit_is_set(UCSR0A, UDRE0);

	// Load buffer with your character

    UDR0 = c;

    return 0;

}

// uart_getchar - Low-level function which waits for the Rx buffer

// to be filled, and then reads one character out of it.

// Note that this function blocks on read - it will wait until

// something fills the Rx buffer.

int serial_getchar(void)

{

	char c;

	// Wait for the receive buffer to be filled

    loop_until_bit_is_set(UCSR0A, RXC0);

	// Read the receive buffer

    c = UDR0;

	return c;

}

// uart_getchar_nb - Low-level function which checks if the Rx buffer

// is filled, and then reads one character out of it.

// This is a non blocking version uart_getchar

int serial_getchar_nb(void)

{

	char c;

	// Wait for the receive buffer to be filled

    //loop_until_bit_is_set(UCSR0A, RXC0);

	if (UCSR0A & _BV(RXC0)){

		// Read the receive buffer

		c = UDR0;

		return c;

	}

	return 0;

}

//setup uart1 (serial1)

void serial1_init( unsigned int ubrr)

{

	/*Set baud rate - baud rates under 4800bps unsupported */