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.

For a high level overview, review the flow chart.

Tasks

Complete

• Got ADC working for color sensor
  • Set up filtering to address projector PWM
• 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 calibration (color_cal) program, to facilitate simple calibration for each e-puck that does not require large amount of data collected for each run, and with minimal user input/control (desire automation and simplification)
• Improve the vision system position information updater
  • So that the e-puck does not automatically assume every packet from the Vision System is true, rather 'checks' for accuracy
• Replace wheelSpeedSingleBot with the three step move controller from NUtest.c
  • Implement this so that the e-puck can be moved with position and orientation (this is ideal so that data from every orientation of the e-puck can be recorded while calibrating or collecting data
• Implement new algorithm from paper

Project Package

The source code for the project is available here:

• 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)

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

Packet Length Constants

These variables determine the length of the XBee packets. See Data Frame and the section on XBee API packets in the XBee manual for further clarification.

Much of this is still sending integral consensus estimator data. This can be removed or replaces with data needed for decentralized color sensing.

#define NUM_DATA_SETS 5

Number of statistics on which you are running the consensus estimator. This this particular case, 5. (Ix, Iy, Ixx, Ixy, Iyy)

#define NUMBERS_PER_SET 2

Number of variables in each data set (see above) that the consensus estimator needs to transmit to other agents. In this case, 2 because there is x_i and w_i for each statistic.

#define DATATYPE_BYTELENGTH 4

Number of bytes in the data type (used in the consensus estimator (float = 4 bytes long). This is important because we need to split the numbers into individual bytes to be able to send them out the serial port.

#define DATA_ARRAY_LENGTH (NUM_DATA_SETS*NUMBERS_PER_SET)

Total number of data variables needed for the consensus estimator. In this case, it is 5*2=10.

#define ADDITIONAL_NUMS 8

Additional number of data to be appended to data array. It is 5 in this case, so that we can append

1. Robot X coordinate
2. Robot Y coordinate
3. Robot Theta orientation
4. Robot left wheel speed
5. Robot right wheel speed
6. Sensor Red Value
7. Sensor Green Value
8. Sensor Blue Value

main(.c/.h)

• .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.

The current handling of coordinate data from the vision system:

case 0: //coordinate data
					e_set_configuration(
					InPacket.data[1].dataFloat,
					InPacket.data[2].dataFloat, 
					InPacket.data[3].dataFloat);
					break;

TO DO

Needs to be improved. Suggested new function:

• Get vision system data
• Check if it's wildly off from current puck estimate
  • If not, update, clear log of rejected packets
  • If so, reject and log
    • If we have rejected enough packets (some threshold) and they've all been in similar place (within tolerance), we can assume the puck is wrong and vision system is right. In this case, update with vision system data and clear rejected data log.

int main(void)

Setup functions and main loop.

color_cal(.c/.h)

• .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)

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

int readPacket( void)

Parses XBee data from UART2. Makes it accessable in the InPacket struct.

The UART2 receive ISR takes error codes from this function and flashes the corresponding puck LED (absolute value of the error code).

Error codes are:

	//////////////ERROR CODES/////////////////
	// 
	// -1: Timeout waiting for UART2
	// -2: Start delimiter wrong
	// -3: Checksum Error
	// -4: UART2 not ready at beginning
	// -5: API_ID unidentified
	// -6: Send Packet CCA failuue
	// -7: Modem Status packet failure
	//////////////////////////////////////////

send_packet(.c/.h)

• .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.

void send_packet(void)

• Creates packet array.
• Adds consensus estimator data to the array.
• Adds robot statistics to the array.
• Adds color sensor values to the array.
• Asserts flow control line to stop XBee from sending
• Calls assemblePacket to send the packet.
• Deasserts flow control; Xbee can send again.

The function e_get_acc_filtered returns a running average of the acc specified. Syntax is:

return value = e_get_acc_filtered(acc_channel, number of samples to average)

The number of samples averaged must be less than ACC_SAMP_NB as defined in e_ad_conv.h.

packet[i]=e_get_acc_filtered(2, 136); // red
	i++;
	packet[i]=e_get_acc_filtered(1, 136); // green
	i++;
	packet[i]=e_get_acc_filtered(0, 136); // blue

PI_consensus_estimator(.h)

• .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/.h)

• .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/.h)

• .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(.c/.h)

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)

• .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. Call before using any ports.

e_led(.c/.h)

• .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)

• .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.

#define POINT_OFFSET -31.75

In addition to constants like wheel radius and wheel base, the offset between the center point of the bot and the point we're driving is set in e_motors_swarm.h. -31.75 mm is the distance betwene the center and the color sensor. So technically, we're driving backwards.

void __attribute__((interrupt, auto_psv, shadow)) _T5Interrupt(void)

Timer5 ISR, interrupt for left motor. Controls the stepper motors.

Updates the pucks x, y, theta estimate here:

// update robot's configuration estimate
	thetapos = thetapos - ANGSTEP;
    if (thetapos<-PI) thetapos += 2*PI;
    xpos = xpos + cval*LINSTEP;
    ypos = ypos + sval*LINSTEP;
    }

void __attribute__((interrupt, auto_psv, shadow)) _T4Interrupt(void)

Timer4 ISR, interrupt for right motor. Controls the stepper motors.

Updates the pucks x, y, theta estimate here:

	// update robot's configuration estimate
	thetapos = thetapos - ANGSTEP;
    if (thetapos<-PI) thetapos += 2*PI;
	xpos = xpos - cval*LINSTEP;
    ypos = ypos - sval*LINSTEP;
    }

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. This is used to overwrite the puck position/orientation estimates with data from the vision system.

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).