View source for Swarm Robot Project E-puck Code

__TOC__
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|Swarm_Robot_Project_Documentation]]

=Project Files=
Project Files: [[Image:swarm_epucks_code.zip]]

=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 [[Swarm_Robot_Project_Documentation#Data_Frame|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 <tt>x_i</tt> and <tt>w_i</tt> 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 
# Robot X coordinate
# Robot Y coordinate
# Robot Theta orientation
# Robot left wheel speed
# 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).

[[Image:e-puck_LED_numbering.png|thumb|left]]

<br clear='all'>

=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)===
Adds new data from incoming packet to runnning sum x_sum.

===void calcNewEstimate(void)===
Calculates a new estimate and resets the x_sum and w_sum running sums.

[[Category:SwarmRobotProject]]
[[Category:e-puck]]