Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include "robot_comm.h"

#include "../common/comm_station_robot.h"

static void send_packet (char type, char *data, int len)

{

	wl_send_global_packet (station_robot_group, type, data, len, 0);

	// TODO: is it guaranteed that the package will be sent on the next call to wl_do, or

	// do we have to call wl_do in a loop?

	wl_do ();

}

void robot_set_orbs (uint8_t red1, uint8_t green1, uint8_t blue1, uint8_t red2, uint8_t green2, uint8_t blue2)

{

	char data[6];

	data[0]=red1;

	data[1]=green1;

	data[2]=blue1;

	data[3]=red2;

	data[4]=green2;

	data[5]=blue2;

	send_packet (station_robot_set_orbs, data, 6);

}

void robot_set_motors (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2)

{

	char data[4];

	data[0]=direction1;

	data[1]=speed1;

	data[2]=direction2;

	data[3]=speed2;

	send_packet (station_robot_set_motors, data, 4);

}

void robot_set_bom (uint16_t bitmask)

{

	char data[2];

	data[0]=WORD_BYTE_0(bitmask);

	data[1]=WORD_BYTE_1(bitmask);

	send_packet (station_robot_set_bom, data, 2);

}

#define num_measurements 5

#define velocity 200

#define delay 500

void test_encoders_direction (uint8_t direction1, uint8_t direction2)

{

	// reset_encoders ();

	for (uint8_t m=0; m<num_measurements; ++m)

	{

		robot_set_motors (direction1, velocity, direction2, velocity);

		delay_ms (delay);

		robot_set_motors (direction1, 0, direction2, 0);

		delay_ms (delay);	// FIXME wait for steady state!

	//	send (turn motor on for time)

	//	wait (done)

	//	wait_stopped

	//	send (read encoders)

	//	receive (data)

	//	read encoders

	}

}

	test_encoders_direction (FORWARD, BACKWARD);

	test_encoders_direction (BACKWARD, FORWARD);

#ifndef _robot_comm_h

#define _robot_comm_h

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

void robot_set_motors (uint8_t direction1, uint8_t speed1, uint8_t direction2, uint8_t speed2);

void robot_set_bom (uint16_t bitmask);

#endif

#include "robot_comm.h"

// NB don't produce overflows!

#define vel_min 160

#define vel_max 250

#define vel_inc 10

#define vel_delay 200

static void test_motors_direction_velocity (uint8_t direction1, uint8_t direction2, uint8_t velocity)

{

	robot_set_motors (direction1, velocity, direction2, velocity);

	delay_ms (vel_delay);

	//	wait (steady)

	//	

	//	reset_encoders ();

	//	for (measurements)

	//	{

	//		measure left/right

	//		wait

	//	}

}

static void test_motors_direction (uint8_t direction1, uint8_t direction2)

{

	// Use 16 bit variable for vel to avoid problems with overflow (if vel=250 and 10 is added, it would be 260-256=4

	// which is still smaller than vel_max). There are more elegant solutions to this problem, but this one is

	// easy and performance is not an issue here anyway.

	for (uint16_t vel=vel_min; vel<=vel_max; vel+=vel_inc)

		test_motors_direction_velocity (direction1, direction2, vel);

	for (uint16_t vel=vel_max; vel>=vel_min; vel-=vel_inc)

		test_motors_direction_velocity (direction1, direction2, vel);

}

	test_motors_direction (FORWARD, BACKWARD);

	test_motors_direction (BACKWARD, FORWARD);

	robot_set_motors (FORWARD, 0, FORWARD, 0);

#include <wireless.h>

#include <xbee.h>

#include "../common/comm_station_robot.h"

#include "test_motors.h"

		usb_puts ("Test (s)elf, (a)ll, (c)omm, (b)om, (r)angefinders, (m)otors, (e)ncoders" NL);

			case 'm': case 'M': if (require_comm ()) test_motors ();        break;

    dragonfly_init(0);

    usb_init ();

    orb_init_pwm ();

	orb1_set (255, 0, 0); usb_puts("Initializing wireless" NL);

	xbee_init ();

	wl_init();

	orb2_set (255, 0, 0); usb_puts("Done" NL);

#include <dragonfly_lib.h>

#include <wireless.h>

#include <xbee.h>

#include "../common/comm_station_robot.h"

static void message_set_orbs (int length, uint8_t *data)

{

	if (length>=6)

	{

		orbs_set (data[0], data[1], data[2], data[3], data[4], data[5]);

	}

}

static void message_set_motors (int length, uint8_t *data)

{

	if (length>=4)

	{

		motor1_set (data[0], data[1]);

		motor2_set (data[2], data[3]);

	}

}

static void message_set_bom (int length, uint8_t *data)

{

	// TODO test

	if (length>=2)

	{

		uint16_t bitmask=WORD(data[0], data[1]);

		bom_set_leds (bitmask);

		if (bitmask==0)

			bom_off ();

		else

			bom_on ();

	}

}

void station_robot_receive(char type, int source, unsigned char* packet, int length)

{

	switch (type)

	{

		case station_robot_set_orbs:   message_set_orbs   (length, packet); break;

		case station_robot_set_motors: message_set_motors (length, packet); break;

		case station_robot_set_bom:    message_set_bom    (length, packet); break;

		// TODO default: error message, red orbs, pause. Test!

		// TODO: error signal in the individual handlers if the number of parameters is too low. Test!

	}

}

int main(void) {

    dragonfly_init(ALL_ON);

    usb_init ();

    usb_puts ("Diagnostic station robot starting up\r\n");

    orb_init_pwm ();

	xbee_init ();

	orb1_set (255, 127, 0);

	wl_init();

	orb2_set (255, 127, 0);

	//usb_puts ("PAN ID: ");  usb_puti (xbee_get_pan_id ());  usb_puts ("\r\n");	// -1

	//usb_puts ("Channel: "); usb_puti (xbee_get_channel ()); usb_puts ("\r\n");	// 0

	//usb_puts ("Address: "); usb_puti (xbee_get_address ()); usb_puts ("\r\n");	// 4/8

	PacketGroupHandler receive_handler = {station_robot_group, NULL, NULL, &station_robot_receive, NULL};

	wl_register_packet_group(&receive_handler);

	while (1)

	{

		wl_do();

	}

}

#ifndef _comm_station_robot_h

#define _comm_station_robot_h

// TODO: define a channel and a PAN (and use it on both the robot and the station)

// The message group (0..15)

#define station_robot_group 1

// The message types

#define station_robot_set_orbs 1

#define station_robot_set_motors 2

#define station_robot_set_bom 3

// TODO: add motors off (and use everywhere)

// TODO: add motors_for_some_time (and use in test_encoders)

#define WORD_BYTE_0(word) ( word    &0xFF)

#define WORD_BYTE_1(word) ((word>>8)&0xFF)

#define WORD(byte0, byte1) (byte1<<8 | byte0)

#endif