Swarm Robot Project E-puck Code

This page documents the e-puck code for the 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).

Go back to Swarm_Robot_Project_Documentation

Description of the files:

• main.c: This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines.
• e_epuck_ports.h: Contains I/O pin definitions.
• main.h: Contains global variables, macros, and the delay function.
• e_init_port.h/.c: Initializes the ports on the e-puck. File is from the standard e-puck library.
• e_led.h/.c: Handes LED manipulation functions. File is from the standard e-puck library.
• e_motors_swarm.h/.c: Motor control and dead reckoning functions. This is a modified version of the standard e_motors.h/.c library file, with dead reckoning functions added.
• dsPIC_XBeePackets.h: Contains functions and data structures for assembling and receiving XBee packets.
• PI_consensus_estimator: Contains functions and data structures for the PI consensus estimator.

main.c

This contains the entry point of the code and contains the initialization routines, main loop, and interrupt service routines.

main(void)

This is the entry point of the code, and contains initialization routines and the main (infinite) loop.

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

This is the interrupt service routine (ISR) for Timer1. This is the "system tick" and triggers every 0.2 seconds. It sets a flag to indicate that the interrupt has been triggered. This is a low priority interrupt.

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

This is the interrupt service routine (ISR) for the UART receiver buffer. This interrupt will trigger whenever a byte comes in on the serial port. This is a high priority interrupt; the PIC will other tasks to run this ISR.

main.h

#define notRTS SELECTOR3

Defines the SELECTOR3 pin (this used to be connected to the selector switch on the original extension module that we have replaced with the XBee board) as the notRTS pin.

#define notCTS SELECTOR2

Defines the SELECTOR2 pin (this used to be connected to the selector switch on the original extension module that we have replaced with the XBee board) as the notCTS pin.

#define SQUARE(x) ((x) * (x))

This macro squares a number.

#define AGENT_ID 1...#if (AGENT_ID == 1)...

This allows you to give the robots an ID number for setting parameters for different robots (change the AGENT_ID and recompile for each robot). This is sometimes useful for testing purposes if you wish to give each agent a different set of initial parameters.

float robotX robotY robotTheta

These global variables hold the (X,Y) coordinates and the orientation (in radians) of the robot (-PI to PI).

Packet Length Variables

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.

#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 5

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

void delay(long time)

General purpose delay function.

e_init_port.h/.c

This is from the standard e-puck library.

e_init_ports(void)

This function sets up the ports on the e-puck.

e_led.h/.c

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

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.

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

wheel_speed_coordinator.h

float MAX_WHEEL_V_TICKS = 500.0

Max velocity in motor steps per second; the larger this is, the less time you have to do other calculations. Limit is probably around 750.

#define XLIMIT 1000.0 /#define YLIMIT 850.0

Test area boundaries. robot will stop if it reaches +/-(XLIMIT) or +/-(YLIMIT).

float deadband=0; deadband value for motor velocity float COMMR = 500000.0; //Max communication radius

static float k[5] = {2.0,2.0,0.00001,0.00001,0.00001}; static float fgoal[5] = {100.0, 300.0, 160000.0, 40000.0, 40000.0};

Variables and functions for obstacle avoidance

float SAFEDIST = 200.0

Distance in mm at which obstacle avoidance routine will start running.

float MINDIST = 100.0

Distance in mm at which obstacle avoidance will dominate the robot's motion.

static volatile float nx=10000.0/static volatile float ny=10000.0

Nearest neighbor's center point position; use a big number so that make sures nx,ny is initialized by the first packet it receives.

static volatile int nid = 100

ID of the nearest neighbor, assume NOBODY will ever be 100 so initialize as this.

static volatile int nflag =0

Flag used just for initialization so we know if it has run yet.

void wheelSpeed(int *vL, int *vR)

This function uses the control law to calculate the desired wheel speeds.

int abs(int input)

Absolute value function for integer data type.

float fabs(float input)

Absolute value function for floating point data type.

PI_consensus_estimator.h

Estimator Gains

float num_agents

Number of agents you've received data from. You must scale the KP and KI gains.

float GAMMA=0.05

Forgetting Factor.

float KP=0.7

Proportional gain. KP=5 will make the system unstable. ===float KI=0.1;

Estimator and Controller Data Arrays

volatile float x_i[NUM_DATA_SETS]

Decision variables.

volatile float u_i[NUM_DATA_SETS]

Controller inputs.

volatile float w_i[NUM_DATA_SETS]

Internal estimator state.

volatile float x_sum[NUM_DATA_SETS]

Running sum of x_i data received from other agents.

volatile float w_sum[NUM_DATA_SETS]

Running sum of w_i data received from other agents.

Message Flags

volatile union MSG_RX_FLAGS

Flags for received messages, so we can keep track of whom we have received messages from. 4-byte long bitfield (32-bits). 31 robots maximum (bit 0 is reserved for base station).

unsigned char getMsgRxFlag(unsigned int flagNum)

Returns value of the particular message flag.

unsigned char setMsgRxFlag(unsigned int flagNum, unsigned int value)

Sets value of particular message flag.

void resetMsgRxFlags(void)

Reset all message flags to 0.

Consensus Estimator Functions

void addConsensusData(void)

//Calculate a new estimate void calcNewEstimate(void){ int j;

   e_get_configuration(&robotX, &robotY, &robotTheta);
   
	u_i[0]=robotX; 
   u_i[1]=robotY;  

u_i[2]=u_i[0]*u_i[0]; u_i[3]=u_i[0]*u_i[1]; u_i[4]=u_i[1]*u_i[1];

for (j=0; j<NUM_DATA_SETS; j++){ x_i[j] = x_i[j] + GAMMA*(u_i[j]-x_i[j]) - (KP/num_agents)*x_sum[j] + (KI/num_agents)*w_sum[j]; w_i[j] = w_i[j] - (KI/num_agents)*x_sum[j]; x_sum[j]=0; w_sum[j]=0; } num_agents = 0; }

1. endif