File content as of revision 5:66e1a4c974dd:

/*******************************************************************************************
 *
 * University of York Robot Lab Pi Swarm Robot Library
 *
 * "Blank" Program
 *
 * Use this file as the template to produce custom controllers
 *
 * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
 * 
 * Version 0.6  February 2014
 *
 * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
 *
 ******************************************************************************************/

#include "main.h"   // Certain parameters can be set by changing the defines in piswarm.h

#define rearSensors 90
#define frontSensors 90
#define wallsSensors 55
#define cohesion 50
#define separation  75

uint8_t loop_counter = 0;
float sensor_data[8] = {0};
volatile uint8_t proximity_signal[4] = {0};
float speed = 0;
float speed_incrementor = 0;
float delay = 0;

DigitalOut irFront(p12);
DigitalOut irBack(p13);
Timer t;
Timer speed_timer;


PiSwarm piswarm;
Serial pc (USBTX,USBRX);

//This is where the program code goes.  In the simple demo, the outer LEDs are blinked.
int main() {
    init();
    t.start();
    speed_timer.start();
    speed = 0.15;
    while(1) {
        
        speed_incrementor = 0.08 + speed / 10;
        delay = 0.12 - (speed / 10) * 4;
        get_sensors_values();
        if( speed_timer.read_ms() > 1000 ){ 
            if( speed > 0.05 ) speed -= 0.005;
            for( loop_counter = 0; loop_counter < 3; loop_counter++ ){
                if( proximity_signal[loop_counter] == 1 && speed < 0.2 ) speed += 0.005; 
            }
            speed_timer.reset();    
            //piswarm.cls();
            //piswarm.printf("%0.2f %d%d%d\n\r", speed, proximity_signal[0], proximity_signal[1], proximity_signal[2]);
        }    
        
        //Forward and backward condition 
        while( ( sensor_data[0] + sensor_data[7] ) / 2 > ( separation + 15 ) )
        {
            attraction();
            piswarm.forward( speed );
            piswarm.set_oleds( 1, 0, 0, 0, 0, 0, 0, 0, 1, 1 );
            get_sensors_values();
            wait( delay );
        }
        while( ( sensor_data[0] + sensor_data[7] ) / 2 < ( separation - 15 ) && ( sensor_data[3] + sensor_data[4] ) / 2 > ( separation - 15 ) )
        {
            piswarm.backward( speed_incrementor );
            piswarm.set_oleds( 0, 0, 0, 1, 1, 1, 0, 0, 0, 0 );
            get_sensors_values();
            wait( delay );
        }
        //Front-left separation
        while( sensor_data[6] < separation || sensor_data[5] < separation )
        {
            t.stop();
            t.reset();
            piswarm.left( speed_incrementor );
            piswarm.set_oleds( 0, 0, 0, 0, 0, 0, 1, 1, 0, 0 );
            get_sensors_values();
            wait( delay );
            t.start();
        }
        
        //Front-right separation
        while( sensor_data[1] < separation || sensor_data[2] < separation)
        {
            t.stop();
            t.reset();
            piswarm.right( speed_incrementor );
            piswarm.set_oleds( 0, 1, 1, 0, 0, 0, 0, 0, 0, 0 );
            get_sensors_values();
            wait( delay );
            t.start();
        } 
        piswarm.stop();     
        attraction();
    }
}

void get_sensors_values()
{
    for(loop_counter = 0; loop_counter < 8; loop_counter++ ){
        sensor_data[loop_counter] = 0.0;
        sensor_data[loop_counter] = piswarm.read_reflected_ir_distance(loop_counter);
    }
}

void attraction(void){
    if(t.read_ms() > 2500){//limit for going without seeing any robot, otherwise check where they are
        t.stop();
        t.reset();
        irFront = 0;
        irBack = 0;
        int maxIrValue = 0;
        int maxIrLoc = 0;
        unsigned short irValue = 0;
        for(int i=0;i<8;i++){
            piswarm.read_adc_value(i);
        }
        for(int i=0;i<8;i++){
            irValue = piswarm.read_adc_value(i);
            if(irValue > maxIrValue){
                maxIrValue = irValue;
                maxIrLoc = i;
            }
        }
        t.start();
        t.reset();
        switch(maxIrLoc){
            case 0:
            piswarm.right(0.36);
            while(t.read_ms() < 30){//15 degrees
            }
            piswarm.stop();
            t.stop();
            break;
            
            case 1:
            piswarm.right(0.36);
            while(t.read_ms() < 90){//45 degrees
            }
            piswarm.stop();
            t.stop();                
            break;

            case 2:
            piswarm.right(0.36);
            while(t.read_ms() < 180){//90 degrees
            }
            piswarm.stop();
            t.stop();                
            break;

            case 3:                
            piswarm.right(0.36);
            while(t.read_ms() < 288){//144 degrees
            }
            piswarm.stop();
            t.stop();
            break;

            case 4:
            piswarm.left(0.36);
            while(t.read_ms() < 288){//216 degrees
            }
            piswarm.stop();
            t.stop();
            break;

            case 5:
            piswarm.left(0.36);
            while(t.read_ms() < 180){//270 degrees
            }
            piswarm.stop();
            t.stop();                
            break;

            case 6:
            piswarm.left(0.36);
            while(t.read_ms() < 90){//315 degrees
            }
            piswarm.stop();
            t.stop();                
            break;

            case 7:
            piswarm.left(0.36);
            while(t.read_ms() < 30){//345 degrees
            }
            piswarm.stop();
            t.stop();                
            break;
        }
        t.start();
        irFront = 1;
        irBack = 1;
    }

}
/***************************************************************************************************************************************
 *
 * Beyond this point, empty code blocks for optional functions is given
 *
 * These may be left blank if not used, but should not be deleted 
 *
 **************************************************************************************************************************************/
 
// Communications

// If using the communication stack (USE_COMMUNICATION_STACK = 1), functionality for handling user RF responses should be added to the following functions
// If the communication stack is not being used, all radio data is sent to processRawRFData() instead

void handleUserRFCommand(char sender, char broadcast_message, char request_response, char id, char is_command, char function, char * data, char length){
    // A 'user' RF Command has been received:  write the code here to process it
    // sender = ID of the sender, range 0 to 31
    // broadcast_message = 1 is message sent to all robots, 0 otherwise
    // request_response = 1 if a response is expected, 0 otherwise
    // id = Message ID, range 0 to 255
    // 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
    // function = The function identifier.  Range 0 to 15
    // * data = Array containing extra data bytes
    // length = Length of extra data bytes held (range 0 to 57)
     proximity_signal[0] = data[0];
     proximity_signal[1] = data[1];
     proximity_signal[2] = data[2];
     piswarm.cls();
     piswarm.printf("%0.2f %d%d%d", speed, proximity_signal[0], proximity_signal[1], proximity_signal[2]);
       
    //Do something...
}    

void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){
    // A 'user' RF Response has been received:  write the code here to process it
    // sender = ID of the sender, range 0 to 31
    // broadcast_message = 1 is message sent to all robots, 0 otherwise
    // success = 1 if operation successful, 0 otherwise
    // id = Message ID, range 0 to 255
    // 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
    // function = The function identifier.  Range 0 to 15
    // * data = Array containing extra data bytes
    // length = Length of extra data bytes held (range 0 to 57)

    //Do something...  
}    

void processRawRFData(char * rstring, char cCount){
    // A raw RF packet has been received: write the code here to process it
    // rstring = The received packet
    // cCount = Packet length
    
    //Do something...
}

void switch_pressed() {
    //Switch(es) pressed {1 = Center  2 = Right  4 = Left  8 = Down  16 = Up}
    char switches = piswarm.get_switches();
  
    //Do something...
}