Swarm Robot Project E-puck Code

From Mech
(Difference between revisions)
Jump to: navigation, search
(=float SAFEDIST = 200.0)
(PI_consensus_estimator.h)

Revision as of 00:30, 21 January 2009

Contents

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-puck LED numbering.png


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;

//Data arrays //DATA_ARRAY_LENGTH and NUM_DATA_SETS are in main.h

volatile float x_i[NUM_DATA_SETS]

Estimator state.

volatile float u_i[NUM_DATA_SETS]

volatile float w_i[NUM_DATA_SETS]={0.0}; volatile float x_sum[NUM_DATA_SETS]={0.0}; //running sum volatile float w_sum[NUM_DATA_SETS]={0.0};


//flags for received messages, so we can keep track of who we have received messages from. //31 robots maximum right now (0 is reserved for base station) volatile union MSG_RX_FLAGS { unsigned long allFlags;//32 bits unsigned char flagBytes[4]; struct{ unsigned flag_0:1; unsigned flag_1:1; unsigned flag_2:1; unsigned flag_3:1; unsigned flag_4:1; unsigned flag_5:1; unsigned flag_6:1; unsigned flag_7:1; unsigned flag_8:1; unsigned flag_9:1; unsigned flag_10:1; unsigned flag_11:1; unsigned flag_12:1; unsigned flag_13:1; unsigned flag_14:1; unsigned flag_15:1; unsigned flag_16:1; unsigned flag_17:1; unsigned flag_18:1; unsigned flag_19:1; unsigned flag_20:1; unsigned flag_21:1; unsigned flag_22:1; unsigned flag_23:1; unsigned flag_24:1; unsigned flag_25:1; unsigned flag_26:1; unsigned flag_27:1; unsigned flag_28:1; unsigned flag_29:1; unsigned flag_30:1; unsigned flag_31:1; }; }MsgRxFlags;

//returns value of message flag unsigned char getMsgRxFlag(unsigned int flagNum){ unsigned int quotient=flagNum/8; //find the byte unsigned int remainder=flagNum%8; //find the bit in the byte unsigned char temp; if (flagNum > 31){ //number is out of range return -1; } temp = MsgRxFlags.flagBytes[quotient]&=(0b00000001<<remainder); if (temp == 0){ return 0; } else { return 1; } }

//sets value of message flag unsigned char setMsgRxFlag(unsigned int flagNum, unsigned int value){ unsigned int quotient=flagNum/8; unsigned int remainder=flagNum%8; if (flagNum > 31){ return -1; } if (value==0){ MsgRxFlags.flagBytes[quotient]&=(0b11111110<<remainder); } else{ MsgRxFlags.flagBytes[quotient]|=(0b00000001<<remainder); } return 1; }

void resetMsgRxFlags(void){ MsgRxFlags.allFlags = 0; }

//adds new data to runnning sum x_sum. //The data in the packet is structured: x_1 w_1 x_2 w_2 x_3 w_3 etc. void addConsensusData(void){ int k; int j=0; for (k=0; k<DATA_ARRAY_LENGTH; ){ x_sum[j]+=x_i[j]-InPacket.data[k].dataFloat; //add (x_i - x_j) to x_sum k++; w_sum[j]+=w_i[j]-InPacket.data[k].dataFloat; j++; k++; } num_agents++; //keep track of number of agents you've received data from. }

//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
Personal tools