RGB Swarm Robot Project E-puck Code (outdated)

This page documents the e-puck code for the RGB Sensing Swarm Robotics project. The code on the e-puck was written in C and compiled using Microchip's MPLAB C Compiler for dsPIC DSCs (student version).

This code is a branch of the Swarm Project E-puck Code.

Tasks

Complete

• Restructured code to make more modular.
  • Split dsPIC_XBeePackets and wheel_speed_coordinator into h and c files
  • Pulled packet assembling code out of main and created send_packet() function in send_packet.h/c.
  • Pulled a bunch of variables and defines (NUM_DATA_SETS, NUMBERS_PER_SET, DATATYPE_BYTELENGTH , DATA_ARRAY_LENGTH , ADDITIONAL_NUMS, notRTS, T1_INT_FLAG, x_i, u_i, w_i, x_sum, w_sum, MAX_WHEEL_V_TICKS, deadband, COMMR, SAFEDIST, MINDIST, u_x_ideal, u_y_ideal, x_motion_integral, y_motion_integral, SQUARE) that were scattered across h files into global_vars.h/c. Makes it easy to include them in a particular file with the extern keyword.
• Added color_cal() function in color_cal.h/c
• Added wheelSpeedSingleBot to wheel_speed_coordinator

To Do

• Finish color_cal
• Make the vision system position information updater smarter.
• Replace wheelSpeedSingleBot with the three step move controller from NUtest.c
• Implement new algorithm

Project Package

The source code for the project is available here: Media:RGB_Swarm_Puck_Code_working_version.zip. Open swarm_epucks.mcw and you should be good to go.

Description of the files and functions

global_vars

• .c/.h: declare and define global variables and macros

main

• .c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines.
• .h: Contains variables, function prototypes, and delay function needed for main.

void __attribute__((__interrupt__,auto_psv)) _T1Interrupt(void)

Timer1 ISR. Sets T1_INT_FLAG which provides timing for the main loop.

void __attribute__((__interrupt__,auto_psv)) _U2RXInterrupt(void)

UART2 receive ISR. Runs with the XBee receives packets. The switch/case structure handles the packets.

int main(void)

Setup functions and main loop.

color_cal

• .c: Contains void calibrate_color(void) function to run the calibration routine.
• .h: Contains function prototype and constant definitions for calibrate_color.

void calibrate_color(void)

This function runs the color calibration routine. Eventually this should be turned into a separate e-puck command from the vision system. Currently it just runs the puck through a zig zag pattern on the floor and sends packets. You can collect these with the data logger and process them in matlab. From this, you should be able to get a calibration function.

To do:

• Add to the pattern. Need to collect more data.
• Possibly have the puck store data, find a best fit, and create the calibration function on board.
• Store the calibration function in the EEPROM (flash memory) so it's non-volatile. The coefficients of function will be specific to each puck, so it would be nice to not have to program each puck, each time you change the battery.

dsPIC_XBeePackets

• .c/.h: Contains functions and data structures for assembling and receiving XBee packets.

send_packet

• .c: Contains the void send_packet(void) function which fills an array with data and calls the needed XBee functions to send a packet.
• .h: Contains function prototype.

PI_consensus_estimator

• .h: Contains functions and data structures for the PI consensus estimator.
• This will probably be replaced by the algorithm for sensor consensus.

wheel_speed_coordinator

• .c: Contains functions for robot motion control
• .h: Function prototypes and variabls.

void wheelSpeed(int *vL, int *vR)

Return needed wheel speeds for the inertial consensus estimator based on the group goal.

void wheelSpeedSingleBot(float gotox, float gotoy, int *vL, int *vR)

Return needed wheel speed to get this individual bot to (gotox, gotoy). It's a hacked fix. Should be replaced with the 3 step motion controller from NUtest.c.

e_acc

• .c: Functions for reading the accelerometers (which is the color sensor).
• .h: Function prototypes.

This is original e-puck library code with the following modifications:

//changed by Sam, July 10, default offset is 2000. we want 0 for RGB sensor.
static int centre_z = 0;			//zero value for z axe

e_ad_conv

Set up the ADCs on the puck. Original e-puck library code, with the following modifications

• .h: Define constants and functional prototypes

MIC_SAMP_FREQ sets the baseline sampling frequency for the ADC, everything else must be a fraction of this. 16384 Hz is the highest possible.

#define MIC_SAMP_FREQ 16384.0	

ACC_PROX_SAMP_FREQ sets the sampling frequency for the accelerometers (color sensor). We found in testing that the puck become non-responsive with this set to 8192 Hz or 16384 Hz.

// sampling frequency for the accelerometres and proximetres
//#define ACC_PROX_SAMP_FREQ 256.0	// WARNING: should be a fraction of MIC_SAMP_FREQ
#define ACC_PROX_SAMP_FREQ 4096	//			to ensure a good timing precision
									// So your options are: 1  2  4  8  16  32  64  128  
									//  256  512  1024  2048  4096  8192  16384

ACC_SAMP_NB is the number of samples to store. We can do an average of up to this many samples. This is set to 140 so we can average 136 samples, which is 4 projector periods.

#define ACC_SAMP_NB  140	// number of accelerometer samples to store

• .c: Functions and interrupt service routines for ADCs. Original e-puck library, no modifications.

e_init_ad_scan(ALL_ADC)

Call to setup ADC and have it work in the background. Use e_acc functions to access data.

e_init_port

• .c/.h: Initializes the ports on the e-puck. File is from the standard e-puck library.

e_init_ports(void)

This function sets up ports on the e-puck.

e_led

• .c/.h: This is a standard e-puck library file that contains functions for manipulating LEDs.

void e_set_led(unsigned int led_number, unsigned int value)

Set led_number (0-7) to value (0=off 1=on higher=inverse).

e_motors_swarm

• .c/.h: This file is a modified version of the e_motors.h e-puck library file. This version keeps track of the robot's position and orientation, and the motor stepping function contains code to update the robot's position when the wheels turn.
• The functions like e_rotate and e_translate have been removed and this version is not dependent on e_agenda.

void e_init_motors(void)

Call this function before other motor functions to initialize the motors.

void e_set_speed_left(int motor_speed)/void e_set_speed_right(int motor_speed)

Set the motor speed in steps/second.

void e_get_configuration(float *xptr, float *yptr, float *thetaptr)

Updates variables with current x, t, and theta (position and orientation) of the center reference point.

void e_set_configuration(float x, float y, float theta)

Sets x, y, theta to values.

void e_get_configuration_front(float *xptr, float *yptr, float *thetaptr)

Updates variables with current x, t, and theta (position and orientation) of the front reference point (used for motor control).