File content as of revision 10:da62735d6df9:

/*******************************************************************************************
 *
 * 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
#include "PiSwarmControllerFunctions.h"


PiSwarm piswarm;
Serial pc (USBTX,USBRX);

//Tickers
Ticker ticker_25ms;
Timer  timer;
Timer timerLevy;

//Global Variables
uint8_t const IR_READ_PER_BEAC = 20; //The number of IR readings between beacon flashes
uint16_t volatile gv_IRVals[IR_READ_PER_BEAC][8] = {0}; //The passive IR values each are stored in this array every 25ms for the last 0.5 seconds
int16_t volatile gv_IRValDiffs[IR_READ_PER_BEAC][8] = {0};
uint8_t volatile beacon_detected[8] = {0}; 
int8_t volatile gv_counter25ms = 0; //This counter is increased every 25ms and resets to zero after one second. (It is signed because 1 is subtracted from it in ReadIRs)
uint8_t const BEACON_SUSPECTED = 50; //Value by which consecutive IR sensor readings need to jump by for in order to cause beacon to be suspected.
uint16_t const AT_BEACON_THRESHOLD = 3700;
uint8_t volatile gv_state = 0; //This is the current state of the finite state machine
uint32_t levyTargetTimeus = 0; //The amount of time in micro seconds by which the robot needs to move in order to reach the distance required in next section of the levy walk.
uint8_t volatile gv_IRDistances[8]; //Using the custom distance function the active ir readings are converted to distances and stored here every 25ms.
int16_t g_currentHeading = 0;
float BASESPEED = 0.4;
int8_t g_beaconOn = 100;
int8_t volatile tickBeaconSuspected = 100; //Is set to the tick value within the period between beacon flashes that the beacon flash is suspected to begin at
int8_t tickBeaconPeriodCheck = 100; //Is used to temporarily store the value of tickBeaconSuspected
uint16_t const TURN_BACK_TIME = (800/BASESPEED); //Time between trying to continiue on path it wason before obstacle given by: (Distance in mm) / (robots speed)
//Flags
int8_t volatile flagSystemState = 0;
int8_t flagObstacle = 0;
int8_t flagStationary = 1; //Used to determine if robot is currentl stationary or moving.
uint8_t flagBeaconSyncronised = 0; //Set to one when the robot is synchronised with the beacon
uint8_t volatile flagBeaconIlluminated = 0; //Should be 1 when beacon is illuminated otherwise 0
uint8_t flagSetNewLevyTime = 1; //Must start as 1
//Ticker Function*************************************************************************************
//This function is called by a ticker every 25ms
//The function firstly stores the background IRS values.
//Secondly if beacon is off it uses illuminated IR sensors to estimate distances
//Each second it will update when it believes beacon illumination time to be
//It will work out which sensors spotted then beacon and will illuminate the Leds accordingly
void readIRs(){
    
    //Fistly update the beaconVisable flag if possible
    if(gv_counter25ms == mod8((g_beaconOn - 1), IR_READ_PER_BEAC)){
        flagBeaconIlluminated = 1;
    }
    
    if(gv_counter25ms == mod8((g_beaconOn + 2), IR_READ_PER_BEAC)){
        flagBeaconIlluminated = 0;
    }
    //Firstly store background values
    //Also make a note of which point in the second did the values change most.
    //For which sensor specifically did the values change the most
    //That sensor will be used to estimate the beacon start time if a threshold value is met
    piswarm.store_background_raw_ir_values();
    int16_t IRchange = 0;
    uint8_t loopCounter = 0;
    //In this loop the raw IR values are read.
    //If the points where the IR values have increased by the greatest amount are noted as this indicates a beacon illumination
    for(loopCounter = 0; loopCounter < 8; loopCounter++) {
        gv_IRVals[gv_counter25ms][loopCounter] = piswarm.get_background_raw_ir_value(loopCounter);
        IRchange = gv_IRVals[gv_counter25ms][loopCounter]-gv_IRVals[mod8((gv_counter25ms-1),IR_READ_PER_BEAC)][loopCounter];
        
        gv_IRValDiffs[gv_counter25ms][loopCounter] = IRchange;
        //printf("change %d count %d\n",IRchange);
        //If difference is greater than a threshold value then the beacon is suspected. This will be confirmed depending on the robots state of movement.
        if (IRchange > BEACON_SUSPECTED){
            tickBeaconSuspected = gv_counter25ms;
            piswarm.cls();
            piswarm.printf("%d",tickBeaconSuspected);
        }        
    }
           
    //Now store the illuminated values if the beacon is not illuminated-
    piswarm.store_illuminated_raw_ir_values();
     
    //In this loop convert each raw active IR reading into a distance estimate   
    for(loopCounter = 0; loopCounter < 8; loopCounter++) {
        
        //Specific sensor readings converted to distances
        float temp = piswarm.get_illuminated_raw_ir_value(loopCounter);
        if(temp>3500){
           temp = 3500;
        } else if (temp < 97){
           temp = 97;
        }
        //#put this into a function 
        //Switch case for robot 5
        switch(loopCounter){
            case 0:
                temp = 662 * sqrt(1/(temp-148));
                break;        
            case 1:
                temp = 662 * sqrt(1/(temp-144));
                break; 
            case 2:
                temp = 662 * sqrt(1/(temp-120));
                break;
            case 3:
                temp = 662 * sqrt(1/(temp-148));
                break;
            case 4:
                temp = 662 * sqrt(1/(temp-120));
                break;
            case 5:
                temp = 662 * sqrt(1/(temp-132));
                break;
            case 6:
                temp = 662 * sqrt(1/(temp-152));
                break;
            case 7:
                temp = 662 * sqrt(1/(temp-212));
                break;
        }
              
        if (temp > 130){
            temp = 130;
        }
        gv_IRDistances[loopCounter] = temp;            

    }     
    //reset counter after 1 second (beacon period)
    gv_counter25ms = mod8(gv_counter25ms + 1,IR_READ_PER_BEAC);
}

//*******************************************************************************************************
//This is where the program code goes.  
int main() {
    init();  
    ticker_25ms.attach_us(&readIRs,25000);
    //starting point in state 11
    timer.start();
    timerLevy.start();
    wait(1); //Wait a second to allow IR array to be filled
    
    //changeState(11);
    
    //Controller is a finite state machine
    while(1){
        
        //Waiting for signal to begin searching
        if(gv_state == 0){
            if(flagSystemState == 1){
                changeState(11);
            }
            wait(1);
            
            //Change state here after recieving a radio command
        //Searching state
        } else if (gv_state == 11 || gv_state == 12){
            
            //Firstly determine if suspected beacon is actually the beacon.
            //This is done by checking the period between flashes matches the beacon period
            if(tickBeaconSuspected != 100){
                //piswarm.stop();
                //When the beacon flag is first raised store its value and reset it
                if(tickBeaconPeriodCheck == 100){
                    tickBeaconPeriodCheck = tickBeaconSuspected;
                    tickBeaconSuspected = 100;
                //Check the timing of the latest jump with the last one to see if period matches the Beacon.
                } else {
                    piswarm.locate(0,1);
                    piswarm.printf("%d %d",tickBeaconPeriodCheck,tickBeaconSuspected);
                    //printf("%d %d *********************************",tickBeaconPeriodCheck,tickBeaconSuspected);
                    //If the two numbers are similar then test will be low. For this to work the period of the ticker and beacon should be the same.
                    int8_t test = (tickBeaconPeriodCheck - tickBeaconSuspected);
                    
                    test = test * test;
                    
                    //if test is low then identify the beacon as the cause of the flags
                    if(test < 2){
                        //Beacon found change to state 2
                        g_beaconOn = tickBeaconPeriodCheck; //update the global variable that stores when beacon flashes occur
                        
                        wait(2);
                        changeState(2);
                    } else {
                        //Reset the flag to try again
                        tickBeaconPeriodCheck = 100;
                    }
                }
            }
            
            if(gv_state == 11){
                //Secondly if obstacles are detected ahead then execute a random turn. 
                if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
                    piswarm.stop();
                    piswarm.cls(); 
                    piswarm.printf("ob R");
                    piswarm.play_tune("CC",1);
                    wait(0.1);
                    flagObstacle = 1;
                    changeState(12);
                } else if(gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
                    piswarm.stop();
                    piswarm.cls(); 
                    piswarm.printf("ob L");
                    piswarm.play_tune("CC",1);
                    wait(0.1);
                    flagObstacle = 2;
                    changeState(12);
                                             
                //Otherwise continue moving forward until distance determined by levy algorithm is calculated. 
                } else if(timerLevy.read_us() > levyTargetTimeus){
                    flagSetNewLevyTime = 1;
                    piswarm.play_tune("G",1);
                    changeState(12);
                    
                } else if (flagStationary == 1){
                               
                    piswarm.forward(BASESPEED);
                    flagStationary = 0;
                }                
                
            } else if(gv_state == 12){
                piswarm.stop();//Stop the robot.
                flagStationary = 1; //update this flag
                int16_t randomAngle;
                //If sent here beacuse of obstacle find angle between -180 to -90 and 90 to 180
                if(flagObstacle == 1){
                    randomAngle = rand()%90 - 135;
                    
                } else if(flagObstacle == 2){
                    randomAngle = rand()%90 + 45;
                    
                //Otherwise if here due to levy walk: turn to any random angle
                } else {
                    randomAngle = rand()%360 - 180;                    
                }
                turnDegrees(randomAngle); //Make the turn
                wait(0.1);
                /* if(gv_IRDistances[0] < 70 || gv_IRDistances[1] < 70 || gv_IRDistances[6] < 70 || gv_IRDistances[7] < 70){
                    //do nothing. Aim is that robot will keep turning the same way until clear of obstacle
                    //could put a count in here to doemergency escape manouvre if necessary
                     piswarm.play_tune("CC",1);
                } else { */
                flagObstacle = 0;
                changeState(11);//Move back into state 11
                      
            }           
            
            
        //Beacon found state    
        } else if (gv_state == 2){
            int16_t maxValue[2] = {0,100};   //Value and sensor position  
            //wait(1);
            uint8_t loopCounter = 0;

            //If beacon visible
            if(flagBeaconSyncronised == 1){
                
                //Firstly check beacon is still visible
                flagBeaconSyncronised = 0;
                //Update array concerning which IRs can see the beacon
                for(loopCounter = 0; loopCounter<8; loopCounter++) {
                                       
                    //Find which sensor has the highest reading         
                    if( gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED) {
                        if(gv_IRVals[g_beaconOn][loopCounter] > maxValue[0]){
                            maxValue[0] = gv_IRVals[g_beaconOn][loopCounter];
                            maxValue[1] = loopCounter;
                        }
                        flagBeaconSyncronised = 1; //This will remain as one so long as at least on sensor can see beacon
                    }
                }
                
                //Only do this if beacon still visible
                if(flagBeaconSyncronised == 1){
                    
                    //If the adjacent two sensors are above the threshold too then they can also be marked as illuminated
                    for(loopCounter = 0; loopCounter<8; loopCounter++){
                        
                        //reset all beacon detected values
                        beacon_detected[loopCounter] = 0;

                        if(abs(maxValue[1] - loopCounter)< 3 || abs(maxValue[1] + 8 - loopCounter)< 3 || abs(maxValue[1] - 8 - loopCounter)< 3) {
                            if(gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED){
                                beacon_detected[loopCounter] = 1;
                            }
                        }
                    }
                
            
                    //Update the piswarm LEDS so the ones that can see the beacon are on.
                    piswarm.set_oleds(beacon_detected[0]||beacon_detected[1],
                        beacon_detected[1]||beacon_detected[2],
                        beacon_detected[2],
                        beacon_detected[3],
                        0,
                        beacon_detected[4],
                        beacon_detected[5],
                        beacon_detected[5]||beacon_detected[6],
                        beacon_detected[6]||beacon_detected[7],
                        beacon_detected[7]||beacon_detected[0]);
                
                    //If the max IR value is below a threshold then move toward beacon. Else change state
                    if(maxValue[0] < AT_BEACON_THRESHOLD){
                        
                        //Calculate the heading of Pi-Swarm Relative to beacon
                        //printf("%d ",g_currentHeading);
                        calculateNewHeading();
                        printf("%d ",g_currentHeading);
                        if(g_currentHeading > 5 ||g_currentHeading < -5){
                            turnDegrees(-g_currentHeading);
                        }
                        printf("%d\n",g_currentHeading);    
                        //}
                        
                        
                        
                        //If the beacon is not currently on but obstacle detected then do obstacle avoidance
                        
                        if(flagBeaconIlluminated == 0){
                            if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
                                turnDegrees(90);
                            } else if (gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
                                turnDegrees(-90);
                            }
                        }
                        
                        
                        piswarm.forward(0.2);
                        wait(1);
                    //Should be at beacon
                    } else {
                        piswarm.stop();
                        changeState(3);
                        
                    }
                }
            //Else need to syncronise with beacon
            } else {
                
                while(flagBeaconSyncronised == 0){  
                    //Sychronise the ticker with the beacon
                    
                    uint8_t testBefore = 0;
                    uint8_t testDuring = 0;
                    uint8_t testAfter = 0;
                    for(loopCounter = 0; loopCounter < 8; loopCounter++){
                        if (gv_IRValDiffs[mod8((g_beaconOn - 1),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){
                            testBefore = 1;
                        }
                        if (gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED){
                            testDuring = 1;
                        }
                        if (gv_IRValDiffs[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){
                            testAfter = 1;
                        }
                        if (gv_IRValDiffs[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] < -BEACON_SUSPECTED){
                            testAfter = 2;
                        }
                    } 
                    //Firstly if the beacon is not detected by any of the sensors then change state back to search
                    if(testBefore == 0 && testDuring == 0 && testAfter == 0){
                        changeState(11);
                        flagBeaconSyncronised = 1;//to exit while loop
                    
                    //If the tick before g_beaconOn is detecting the change caused by the flash change the value of g_beaconOn 
                    } else if(testBefore == 1){
                        g_beaconOn = g_beaconOn - 1;
                    
                    //If the After Tick does not show a drop in value then it is also occuring within the beacon flash so delay the ticker by 15ms
                    } else if(testBefore == 0 && testDuring == 1 && testAfter == 1){
                        ticker_25ms.detach();
                        timer.reset();
                        while(timer.read_ms() < 100){};
                        ticker_25ms.attach_us(&readIRs,25000);
                        wait(1);
                    
                    //If successful the set flag
                    } else if (testBefore == 0 && testDuring == 1 && testAfter == 2){
                        flagBeaconSyncronised = 1;
                    
                    //Error handle. If this happens stop the piswarm
                    } else {
                        piswarm.set_oled_colour(255,255,255);
                        piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
                        piswarm.cls();
                        piswarm.printf("%d %d %d",testBefore, testDuring,testAfter);
                        piswarm.stop();
                        wait(5);
                        changeState(11);
                    }
                }
            }              
        }
    }  
}

/***************************************************************************************************************************************
 *
 * 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)
    
       
    //Do something...
    if(function == 1 && gv_state == 0) {
        flagSystemState = 1;
    }
}    

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