main.cpp

00001 /*******************************************************************************************
00002  *
00003  * University of York Robot Lab Pi Swarm Robot Library
00004  *
00005  * "Blank" Program
00006  *
00007  * Use this file as the template to produce custom controllers
00008  *
00009  * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
00010  * 
00011  * Version 0.6  February 2014
00012  *
00013  * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
00014  *
00015  ******************************************************************************************/
00016 
00017 #include "main.h"   // Certain parameters can be set by changing the defines in piswarm.h
00018 
00019 //Define constants
00020 #define delay 0.05
00021 #define rearSensors 90
00022 #define frontSensors 90
00023 #define wallsSensors 55
00024 #define cohesion 45
00025 #define separation  85
00026 
00027 //Define global variables
00028 uint8_t loop_counter = 0;
00029 float sensor_data[8] = {0};
00030 uint8_t noise_indicator = 0;
00031 float speed = 0.075;
00032 
00033 DigitalOut irFront(p12);
00034 DigitalOut irBack(p13);
00035 Timer t;
00036 
00037 
00038 PiSwarm piswarm;
00039 Serial pc (USBTX,USBRX);
00040 
00041 //This is where the program code goes.  In the simple demo, the outer LEDs are blinked.
00042 int main() {
00043     init();
00044     t.start();
00045     while(1) {
00046         if ( noise_indicator == 1) flocking();
00047         else aggregation();
00048     }
00049 }
00050 //This function collects all outer IR sensors information and store them in an array.
00051 void get_sensors_values()
00052 {
00053     for(loop_counter = 0; loop_counter < 8; loop_counter++ ){
00054         sensor_data[loop_counter] = 0.0;
00055         sensor_data[loop_counter] = piswarm.read_reflected_ir_distance(loop_counter);
00056     }
00057 }
00058 //Detect strongest source of IR and face towards it. Cosntant defines rate of spin.
00059 void attraction( float direction_constant ){
00060     if(t.read_ms() > 2500){//limit for going without seeing any robot, otherwise check where they are
00061         t.stop();
00062         t.reset();
00063         irFront = 0;
00064         irBack = 0;
00065         int maxIrValue = 0;
00066         int maxIrLoc = 0;
00067         unsigned short irValue = 0;
00068         for(int i=0;i<8;i++){
00069             piswarm.read_adc_value(i);
00070         }
00071         for(int i=0;i<8;i++){
00072             irValue = piswarm.read_adc_value(i);
00073             if(irValue > maxIrValue){
00074                 maxIrValue = irValue;
00075                 maxIrLoc = i;
00076             }
00077         }
00078         t.start();
00079         t.reset();
00080         switch(maxIrLoc){
00081             case 0:
00082             piswarm.right(0.36);
00083             while(t.read_ms() < 30){//15 degrees
00084             }
00085             piswarm.stop();
00086             t.stop();
00087             break;
00088             
00089             case 1:
00090             piswarm.right(0.36);
00091             while(t.read_ms() < 90){//45 degrees
00092             }
00093             piswarm.stop();
00094             t.stop();                
00095             break;
00096 
00097             case 2:
00098             piswarm.right(0.36);
00099             while(t.read_ms() < 180){//90 degrees
00100             }
00101             piswarm.stop();
00102             t.stop();                
00103             break;
00104 
00105             case 3:                
00106             piswarm.right(0.36);
00107             while(t.read_ms() < 288){//144 degrees
00108             }
00109             piswarm.stop();
00110             t.stop();
00111             break;
00112 
00113             case 4:
00114             piswarm.left(0.36);
00115             while(t.read_ms() < 288){//216 degrees
00116             }
00117             piswarm.stop();
00118             t.stop();
00119             break;
00120 
00121             case 5:
00122             piswarm.left(0.36);
00123             while(t.read_ms() < 180){//270 degrees
00124             }
00125             piswarm.stop();
00126             t.stop();                
00127             break;
00128 
00129             case 6:
00130             piswarm.left(0.36);
00131             while(t.read_ms() < 90){//315 degrees
00132             }
00133             piswarm.stop();
00134             t.stop();                
00135             break;
00136 
00137             case 7:
00138             piswarm.left(0.36);
00139             while(t.read_ms() < 30){//345 degrees
00140             }
00141             piswarm.stop();
00142             t.stop();                
00143             break;
00144         }
00145         t.start();
00146         irFront = 1;
00147         irBack = 1;
00148     }
00149 }
00150 
00151 //Do flocking behaviour
00152 void flocking( void ){
00153     get_sensors_values();
00154     //Forward and backward condition 
00155     while( ( sensor_data[0] + sensor_data[7] ) / 2 > separation && sensor_data[1] > cohesion && sensor_data[6] > cohesion && sensor_data[5] > cohesion && sensor_data[2] > cohesion)
00156     {
00157         attraction( 0.5 );
00158         piswarm.forward( speed );
00159         piswarm.set_oleds( 1, 0, 0, 0, 0, 0, 0, 0, 1, 1 );
00160         get_sensors_values();
00161         wait( delay );
00162     }
00163     while( ( sensor_data[0] + sensor_data[7] ) / 2 < cohesion && ( sensor_data[3] + sensor_data[4] ) / 2 > cohesion )
00164     {
00165         piswarm.backward( speed );
00166         piswarm.set_oleds( 0, 0, 0, 1, 1, 1, 0, 0, 0, 0 );
00167         get_sensors_values();
00168         wait( delay );
00169     }
00170     //Front-left separation
00171     while( sensor_data[6] < cohesion || sensor_data[5] < cohesion )
00172     {
00173         t.stop();
00174         t.reset();
00175         piswarm.right( speed );
00176         piswarm.set_oleds( 0, 0, 0, 0, 0, 0, 1, 1, 0, 0 );
00177         get_sensors_values();
00178         wait( delay );
00179         t.start();
00180     }
00181     
00182     //Front-right separation
00183     while( sensor_data[1] < separation || sensor_data[2] < cohesion)
00184     {
00185         t.stop();
00186         t.reset();
00187         piswarm.left( speed );
00188         piswarm.set_oleds( 0, 1, 1, 0, 0, 0, 0, 0, 0, 0 );
00189         get_sensors_values();
00190         wait( delay );
00191         t.start();
00192     } 
00193     piswarm.stop();     
00194     attraction( 1.0 );
00195 }
00196 //Do aggregation behaviour
00197 void aggregation( void ){
00198     get_sensors_values();
00199     //Forward and backward condition 
00200     while( ( sensor_data[0] + sensor_data[7] ) / 2 > separation && sensor_data[1] > cohesion && sensor_data[6] > cohesion && sensor_data[5] > cohesion && sensor_data[2] > cohesion)
00201     {
00202         attraction( 0.5 );
00203         piswarm.forward( speed );
00204         piswarm.set_oleds( 1, 0, 0, 0, 0, 0, 0, 0, 1, 1 );
00205         get_sensors_values();
00206         wait( delay );
00207     }
00208     /*while( ( sensor_data[0] + sensor_data[7] ) / 2 < cohesion && ( sensor_data[3] + sensor_data[4] ) / 2 > cohesion )
00209     {
00210         piswarm.backward( speed );
00211         piswarm.set_oleds( 0, 0, 0, 1, 1, 1, 0, 0, 0, 0 );
00212         get_sensors_values();
00213         wait( delay );
00214     }*/
00215     //Front-left separation
00216     while( sensor_data[6] < cohesion || sensor_data[5] < cohesion )
00217     {
00218         t.stop();
00219         t.reset();
00220         piswarm.left( speed );
00221         piswarm.set_oleds( 0, 0, 0, 0, 0, 0, 1, 1, 0, 0 );
00222         get_sensors_values();
00223         wait( delay );
00224         t.start();
00225     }
00226     
00227     //Front-right separation
00228     while( sensor_data[1] < separation || sensor_data[2] < cohesion)
00229     {
00230         t.stop();
00231         t.reset();
00232         piswarm.right( speed );
00233         piswarm.set_oleds( 0, 1, 1, 0, 0, 0, 0, 0, 0, 0 );
00234         get_sensors_values();
00235         wait( delay );
00236         t.start();
00237     } 
00238     piswarm.stop();     
00239     attraction( 1.0 );
00240 }    
00241 /***************************************************************************************************************************************
00242  *
00243  * Beyond this point, empty code blocks for optional functions is given
00244  *
00245  * These may be left blank if not used, but should not be deleted 
00246  *
00247  **************************************************************************************************************************************/
00248  
00249 // Communications
00250 
00251 // If using the communication stack (USE_COMMUNICATION_STACK = 1), functionality for handling user RF responses should be added to the following functions
00252 // If the communication stack is not being used, all radio data is sent to processRawRFData() instead
00253 
00254 void handleUserRFCommand(char sender, char broadcast_message, char request_response, char id, char is_command, char function, char * data, char length){
00255     // A 'user' RF Command has been received:  write the code here to process it
00256     // sender = ID of the sender, range 0 to 31
00257     // broadcast_message = 1 is message sent to all robots, 0 otherwise
00258     // request_response = 1 if a response is expected, 0 otherwise
00259     // id = Message ID, range 0 to 255
00260     // is_command = 1 is message is a command, 0 if it is a request.  If RF_ALLOW_COMMANDS is not selected, only requests will be sent to this block
00261     // function = The function identifier.  Range 0 to 15
00262     // * data = Array containing extra data bytes
00263     // length = Length of extra data bytes held (range 0 to 57)
00264       
00265     //Do something...
00266     uint8_t speed_digit_1 = data[2] - '0';
00267     uint8_t speed_digit_2 = data[3] - '0';
00268     speed = speed_digit_1 * 0.1 + speed_digit_2 * 0.01;
00269     noise_indicator = data[4];
00270     piswarm.cls();
00271     piswarm.printf("%0.2f %i", speed, noise_indicator);
00272 }    
00273 
00274 void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){
00275     // A 'user' RF Response has been received:  write the code here to process it
00276     // sender = ID of the sender, range 0 to 31
00277     // broadcast_message = 1 is message sent to all robots, 0 otherwise
00278     // success = 1 if operation successful, 0 otherwise
00279     // id = Message ID, range 0 to 255
00280     // is_command = 1 is message is a command, 0 if it is a request.  If RF_ALLOW_COMMANDS is not selected, only requests will be sent to this block
00281     // function = The function identifier.  Range 0 to 15
00282     // * data = Array containing extra data bytes
00283     // length = Length of extra data bytes held (range 0 to 57)
00284 
00285     //Do something...  
00286 }    
00287 
00288 void processRawRFData(char * rstring, char cCount){
00289     // A raw RF packet has been received: write the code here to process it
00290     // rstring = The received packet
00291     // cCount = Packet length
00292     
00293     //Do something...
00294 }
00295 
00296 void switch_pressed() {
00297     //Switch(es) pressed {1 = Center  2 = Right  4 = Left  8 = Down  16 = Up}
00298     char switches = piswarm.get_switches();
00299   
00300     //Do something...
00301 }