Colony

#ifndef _CIRCLE_H

#define _CIRCLE_H

#include <inttypes.h>

/**** This file should not be edited! ****/

/*

 * The packet structure is 2 bytes

 * byte 0 is the action, which is one of the values below

 * byte 1 is the robot id

 */

#define CIRCLE_ACTION_EXIST 'E'

#define CIRCLE_ACTION_POSITION 'P'

#define CIRCLE_ACTION_ACK 'A'

#define CIRCLE_ACTION_DONE 'D'

//#define EDGE 0;

//#define BEACON 1;

#endif

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include <encoders.h>

#include "hive.h"

int timeout = 0;

int sending = 0;

int desired_max_bom;

int bom_max_counter;

//Last used 12,13,7(BOM)

void set_desired_max_bom(int desired_angle)

{

    if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0;

    if (desired_angle >= 11 && desired_angle < 33) desired_max_bom = 1;

    if (desired_angle >= 33 && desired_angle < 56) desired_max_bom = 2;

    if (desired_angle >= 56 && desired_angle < 78) desired_max_bom = 3;

    if (desired_angle >= 78 && desired_angle < 101) desired_max_bom = 4;

    if (desired_angle >= 101 && desired_angle < 123) desired_max_bom = 5;

    if (desired_angle >= 123 && desired_angle < 145) desired_max_bom = 6;

    if (desired_angle >= 145 && desired_angle < 167) desired_max_bom = 7;

    if (desired_angle >= 167 && desired_angle < 190) desired_max_bom = 8;

    if (desired_angle >= 190 && desired_angle < 212) desired_max_bom = 9;

    if (desired_angle >= 212 && desired_angle < 235) desired_max_bom = 10;

    if (desired_angle >= 235 && desired_angle < 257) desired_max_bom = 11;

    if (desired_angle >= 257 && desired_angle < 280) desired_max_bom = 12;

    if (desired_angle >= 280 && desired_angle < 302) desired_max_bom = 13;

    if (desired_angle >= 302 && desired_angle < 325) desired_max_bom = 14;

    if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15;

}

void forward(int speed){			// set the motors to this forward speed.

	motor_l_set(FORWARD,speed);

	motor_r_set(FORWARD,speed);

}

void left(int speed){				// turn left at this speed.

	motor_l_set(FORWARD,speed);

	motor_r_set(BACKWARD,speed);

}

void right(int speed){

	motor_l_set(BACKWARD,speed);

	motor_r_set(FORWARD,speed);

}

void stop(void){					// could be set to motors_off(), or just use this as an alternative.

	motor_l_set(BACKWARD,0);			// stop() is better - motors_off() creates a slight delay to turn them back on.

	motor_r_set(FORWARD,0);

}

void setforward(int spd1, int spd2){

	motor_l_set(FORWARD,spd1);

	motor_r_set(FORWARD,spd2);

}

void backward(int speed){

	motor_l_set(BACKWARD, speed);

	motor_r_set(BACKWARD, speed);

}

void switch_sending(void)

{

    if(sending)

    {

	sending = 0;

	bom_off();

    }

    else

    {

	sending = 1;

	bom_on();

    }

}

int get_distance(void){				// takes an averaged reading of the front rangefinder

    int temp,distance,kk=5;			// kk sets this to 5 readings.

    distance =0;

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

	temp = range_read_distance(IR2);

	if (temp == -1)

	{

	    //temp=0;

	    i--;

	}

	else

	    distance+= temp;

	delay_ms(3);

    }

    if (kk>0)

	return (int)(distance/kk);

    else 

	return 0;

}

/*

  Orients the robot so that it is facing the beacon (or the broadcasting BOM).

*/

void correctTurn(void)

{

    orb1_set_color(BLUE);			// BLUE and PURPLE

    left(220);

    while(1)

    {

	int bomNum = 0;				// bomNum is the current maximum reading

	bom_refresh(BOM_ALL);

	bomNum = bom_get_max();

	usb_puti(bomNum);

	if(bomNum == 4)				// when it's turned the right way, stop

	{

	    timeout = 0;

	    //motor_l_set(1, 200);

	    //motor_r_set(1, 200);

	    break;				// exits the while() loop to stop the method

	}

	else					// facing the wrong way

	{

	    if(bomNum == -1)

	    {

		timeout++;

		if(timeout > 5000)	// if it's been looking too long, move a little bit as it turns

		{

		    motor_r_set(FORWARD, 210);

		    motor_l_set(BACKWARD, 190);

		}

	    }

	    else if((bomNum >= 12) || (bomNum < 4))

	    {

		motor_l_set(FORWARD, 200);

		motor_r_set(BACKWARD, 200);

		timeout = 0;

	    }

	    else

	    {

		motor_l_set(BACKWARD, 200);

		motor_r_set(FORWARD, 200);

		timeout = 0;

	    }

	}

    }

    return;

}

/* 

   drives forward a hardcoded distance. May not be useful.

*/

void go_straight(void){

    forward(200);

    encoder_rst_dx(LEFT);

    encoder_rst_dx(RIGHT);

    delay_ms(100); 

    int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);

    int count = 0;

    int d;

    while (count<25){						//count = 25 when bot6; count <12

	x_left = encoder_get_x(LEFT);

	x_right = encoder_get_x(RIGHT);

	d = x_right-x_left;

	if (d>13 || d<-13){

	    if(d<50 && d>-50){

		d = round(1.0*d/4);

		setforward(200+d, 200-d);

	    }

	}

	delay_ms(32);

	count++;

    }

}

/* 

   BLINK the given number times

*/

void blink(int num) {

    for(int i = 0; i<num; i++)

    {

	orb_set_color(ORB_OFF);

	delay_ms(350);

	orb_set_color(RED);

	delay_ms(200);

    }

    orb_set_color(ORB_OFF);

}

int main(void)

{

    /* Initialize dragonfly board */

    dragonfly_init(ALL_ON);

    /* Initialize the basic wireless library */

    wl_basic_init_default();

    /* Set the XBee channel to your assigned channel */	/* Set the XBee channel to your assigned channel */

    wl_set_channel(24);

    orb_set_color(RED);

    while(1); /* END HERE */

    //return 0;

}

# this is a local makefile

# Relative path to the root directory (containing lib directory)

ifndef COLONYROOT

COLONYROOT := ..

# Target file name (without extension).

TARGET = hive

# Uncomment this to use the wireless library

USE_WIRELESS = 1

# com1 = serial port. Use lpt1 to connect to parallel port.

AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)

#AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)

else

COLONYROOT := ../$(COLONYROOT)

endif

include $(COLONYROOT)/Makefile