File content as of revision 17:4fa7c9e1b512:

/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2015, University of York Robotics Laboratory (YRL).
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
/**
* @file main.cpp
* @brief The Ticker fucntion for gaining sensor data and the main function for the PRGP Pi-Swarm Controllers.
* @details In this file the main function for the Pi-Swarm Controller is defined.
* @version 1.0
* @author Robert Evans
* @date 24/07/15
* @version 2.0
* @author Homero Silva
* @date 16/08/15
* @ultrasonic sensor added
* @version 3.0
* @author Robert Evans
* @date 17/08/15
* @communication added
*/
#include "main.h"   // Certain parameters can be set by changing the defines in piswarm.h
#include "pi_swarm_functions.h"
#include "hcsr04.h"

PiSwarm piswarm;
Serial pc (USBTX,USBRX);
HCSR04 usensor(p30,p14);

//Tickers
Ticker ticker_25ms; //Ticker used to periodically obtain new IR readings.
Ticker ticker_ultrasonic50ms; //Ticker used to periodically obtain a new ultrasonic reading.

//Timeouts
Timeout tickChangeTimeout; //Timeout used to adjust IR read ticker phase to enable to beacon syncronisation.
Timeout broadcastTimeout; //Timeout used for broadcasting messages multiple times with a set interval.
Timeout stopSearchTimeout;

//Timers
Timer turn_timer; //Timer used to turn a user specified number of degrees.
Timer levy_timer; //Timer used to control time between levy walk steps.
Timer back_move_timer; //Timer used to make the piswarm move back and turn periodically in case it is stuck moving to the beacon.

//Global Variables

//Individucal Pi-Swarm variables: speed and motor trim etc
float BASE_SPEED = 0.6; //The input to the motor functions when the robot is moving forward and does not detect obstacles.
float r_mot_scaler = 1; //This variable is allows particular pi swarms to be adjusted to travel in a straight line.
float l_mot_scaler = 1; //This variable is allows particular pi swarms to be adjusted to travel in a straight line.
uint16_t at_beacon_threshold = 3800; //A raw IR reading greater than this value indicates the robot is at a beacon.
int16_t g_currentHeading = 0; //The current heading of the Pi-swarm relative to the IR beacon.
uint8_t robot_id; //The id unique to each Pi-Swarm robot

//Finite state machine information
uint8_t volatile gv_state = States::READY_TO_START; //This is the current state of the finite state machine
int8_t g_obstacle_type = 0; //The Pi-Swarm will perform different obstacle avoidance behaviours depending on which sensors detect obstacles. This variable is used to note which sensor detected the obstacle.
uint8_t consecutive_turn_count = 0;
uint8_t volatile stop_search_flag = 0; //Set to one ten seconds after command five received. Means stop searching for target beacon.
uint8_t volatile sync_attempt_count = 0;

//IR Readings + beacon syncronisation 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};//The difference between consective IR readings.
int16_t volatile gv_IRValDiffsTwo[IR_READ_PER_BEAC][8] = {0};//The difference between IR readings seperated by two ticks
uint8_t volatile beacon_detected[8] = {0}; //An IR sensor is judged to have a detected the beacon in a valid way the corresponding array variable will be set to 1, else 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)
int8_t g_beaconOn = 100; //The first tick at which the beacon is illuminated
uint8_t const BEACON_SUSPECTED = 20; //Value by which consecutive IR sensor readings need to jump by for in order to cause beacon to be suspected.
uint8_t volatile gv_IRDistances[8]; //Using the custom distance function the active IR readings are converted to distances and stored here every 25ms.
int8_t volatile tick_beacon_suspected = 100; //Is set to the tick value within the period between beacon flashes that the beacon flash is suspected to begin at
int8_t tick_beacon_period_check = 100; //Is used to temporarily store the value of tick_beacon_suspected
uint8_t beacon_syncronised_flag = 0; //Set to one when the robot is synchronised with the beacon
uint8_t volatile beacon_illuminated_flag = 0; //Should be 1 when beacon is illuminated otherwise 0

//Ultrasonic Readings
float volatile distance_ultrasonic_sensor = 1000; //Here is stored the latest reading from the ultrasonic sensor in mm from 1 -800. 1000 is an error value.

//Levy walk variables
uint32_t levy_target_time_us = 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 g_back_count = 0; //After a set number of Levy walk turns the robot will move backward. This variable monitors how many Levy turns since the last backward move.
uint8_t set_new_levy_time_flag = 1; //Must start as 1. If fwd move state is entered following a levy turn the flag will be 1 indicating a new time should be set.


//Communication
uint8_t broadcasted_flag = 1; //Set to one when a message has been broadcast. 0 when about to broadcast a new message
uint8_t broadcasted_count = 0; //The number of messages that have been broadcasted in one batch
int8_t volatile start_flag = 0; //Set to 1 on receiving a command by RF. Indicates robot can start searching for target beacon.
int8_t volatile back_flag = 0;  //Set to 1 on receiving a command by RF. Indicates robot can start searching for home beacon, unless it is at the target beacon.
int8_t volatile return_flag = 0; //Set to 1 on receiving a command by RF. Indicates all robots now start or continue searching for home beacon.

//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)) {
        beacon_illuminated_flag = 1;
    }

    if(gv_counter25ms == mod8((g_beaconOn + 2), IR_READ_PER_BEAC)) {
        beacon_illuminated_flag = 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;
    int16_t IRchange2 = 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];
        IRchange2 = gv_IRVals[gv_counter25ms][loopCounter]-gv_IRVals[mod8((gv_counter25ms-2),IR_READ_PER_BEAC)][loopCounter];
        gv_IRValDiffs[gv_counter25ms][loopCounter] = IRchange;
        gv_IRValDiffsTwo[gv_counter25ms][loopCounter] = IRchange2;

        //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) {
            tick_beacon_suspected = gv_counter25ms;
            piswarm.cls();
            piswarm.printf("%d",tick_beacon_suspected);
        }
    }

    //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(gv_counter25ms == 1) pc.printf("sen %d :%f\n", loopCounter,temp);
        if(gv_counter25ms == 0) {
            pc.printf("sen %d raw: %f",loopCounter,temp);
        }
        if(temp>3500) {
            temp = 3500;
        } else if (temp < 97) {
            temp = 97;
        }
        //#put this into a function
        //Switch case for robot 5
        switch(robot_id) {
            case 9:
                switch(loopCounter) {
                    case 0:
                        temp = 1643 * sqrt(1/(temp-190))-35;
                        break;
                    case 1:
                        temp = 1097 * sqrt(1/(temp-190))-24;
                        break;
                    case 2:
                        temp = 838 * sqrt(1/(temp-120))-17;
                        break;
                    case 3:
                        temp = 1017 * sqrt(1/(temp-175))-22;
                        break;
                    case 4:
                        temp = 777 * sqrt(1/(temp-130))-16;
                        break;
                    case 5:
                        temp = 765 * sqrt(1/(temp-115))-11;
                        break;
                    case 6:
                        temp = 1086 * sqrt(1/(temp-150))-17;
                        break;
                    case 7:
                        temp = 1578 * sqrt(1/(temp-220))-24;
                        break;
                }
                break;
            case 12:
                switch(loopCounter) {
                    case 0:
                        temp = 877 * sqrt(1/(temp-80))-13;
                        break;
                    case 1:
                        temp = 887 * sqrt(1/(temp-110))-13;
                        break;
                    case 2:
                        temp = 744 * sqrt(1/(temp-90))-8;
                        break;
                    case 3:
                        temp = 816 * sqrt(1/(temp-105))-17;
                        break;
                    case 4:
                        temp = 821 * sqrt(1/(temp-125))-7;
                        break;
                    case 5:
                        temp = 741 * sqrt(1/(temp-110))-7;
                        break;
                    case 6:
                        temp = 1000 * sqrt(1/(temp-95))-18;
                        break;
                    case 7:
                        temp = 924 * sqrt(1/(temp-115))-12;
                        break;
                }
                break;
            default:
                temp = 662 * sqrt(1/(temp-152));

        }

        if(gv_counter25ms == 0) {
            pc.printf("sen %d dist:%f\n",loopCounter,temp);
        }
        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);
}

//Get the distance of the ultrsonic sensor in cm
void get_ultrasonic_readings()
{
    float old_dist = distance_ultrasonic_sensor;
    int static count = 0;
    distance_ultrasonic_sensor = usensor.get_dist_mm();
    if(count <100) {
        //pc.printf("US: %.1f, %d\n",distance_ultrasonic_sensor, count);
        count ++;
    } else {
        count =0;
    }

    if(distance_ultrasonic_sensor <= 0 || distance_ultrasonic_sensor == 1000) {
        distance_ultrasonic_sensor = old_dist;
    }
    usensor.start();
}

//When called the readIRs ticker will be reattached after the specified time.
void atTimeout()
{
    ticker_25ms.attach_us(&readIRs,25000);
}

void atBroadcastTimeout()
{
    char function;
    if(robot_id == 1|| robot_id == 9) {
        function = 2;
    } else if(robot_id == 12) {
        function = 3;
    }

    char message[2];
    message[0] = piswarm.get_id()+48;
    broadcast_user_rf_command(function,message,1);
    broadcasted_count++;
    broadcasted_flag = 1;
}

void atStopSearchTimeout(){
    stop_search_flag = 1;
}
//This is a wait function for one

//*******************************************************************************************************
//This is where the program code goes.
int main()
{
    init();
    robot_id = piswarm.get_id();
    //starting point in state 11
    //usensor.start();  // get first reading of the ultrasonic sensor required before ticker
    //wait_ms(50);


    //wait(1); //Wait a second to allow IR array to be filled

    //Controller is a finite state machine
    while(1) {

        //Waiting for signal to begin searching
        if(gv_state == States::READY_TO_START) {
            //pc.printf("%d\n",start_flag);
            if(start_flag == 1) {
                //Change state here after recieving a radio command
                ticker_25ms.attach_us(&readIRs,25000);

                if(robot_id == 1) {
                    at_beacon_threshold = 3850;
                }

                else if (robot_id == 9) {
                    at_beacon_threshold = 3925;
                }

                else if (robot_id == 12) {
                    at_beacon_threshold = 3825;
                    r_mot_scaler = 1.03;
                    l_mot_scaler = 0.97;
                }

                else {
                    at_beacon_threshold = 3900;
                }

                usensor.start();  // get first reading of the ultrasonic sensor required before ticker
                wait_ms(50);
                ticker_ultrasonic50ms.attach_us(&get_ultrasonic_readings,50000);

                turn_timer.start();
                levy_timer.start();
                //pc.printf("why\n");
                changeState(States::SEARCHING_FWD);
            }

            //Searching state
        } else if (gv_state == States::SEARCHING_FWD || gv_state == States::SEARCHING_TURNING) {

            //Do something here on receipt of 'function 5' if necessary.
            //As currently the home beacon will immediately switch on that is not necessary.

            //Determine if suspected beacon is actually the beacon.
            //This is done by checking the period between flashes matches the beacon period
            if(tick_beacon_suspected != 100) {
                //When the beacon flag is first raised store its value and reset it
                if(tick_beacon_period_check == 100) {
                    tick_beacon_period_check = tick_beacon_suspected;
                    tick_beacon_suspected = 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",tick_beacon_period_check,tick_beacon_suspected);
                    //printf("%d %d *********************************",tick_beacon_period_check,tick_beacon_suspected);
                    //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 = (tick_beacon_period_check - tick_beacon_suspected);

                    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 = tick_beacon_period_check; //update the global variable that stores when beacon flashes occur

                        //wait(2);
                        changeState(States::MOVING_TO_BEACON);
                    } else {
                        //Reset the flag to try again
                        tick_beacon_period_check = 100;
                    }
                }
            }

            if(gv_state == States::SEARCHING_FWD) {

                if(stop_search_flag == 1 && back_flag == 0){
                    piswarm.stop();
                    piswarm.set_oled_colour(0,255,255);
                    ticker_25ms.detach();
                    ticker_ultrasonic50ms.detach();
                    while(back_flag == 0){
                        
                    }
                    
                    ticker_25ms.attach_us(&readIRs,25000);
                    ticker_ultrasonic50ms.attach_us(&get_ultrasonic_readings,50000);
                   
                
                } else if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100) {
                    piswarm.stop();
                    piswarm.cls();
                    piswarm.printf("ob R");
                    piswarm.play_tune("CC",1);
                    g_obstacle_type = 1;
                    changeState(States::SEARCHING_TURNING);

                    //Avoid obstacle to the left
                } else if(gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100) {
                    piswarm.stop();
                    piswarm.cls();
                    piswarm.printf("ob L");
                    piswarm.play_tune("CC",1);
                    g_obstacle_type = 2;
                    changeState(States::SEARCHING_TURNING);

                } else if(distance_ultrasonic_sensor < 70) {
                    piswarm.stop();
                    piswarm.cls();
                    piswarm.printf("ob F");
                    piswarm.play_tune("DD",1);
                    //wait(0.1);
                    g_obstacle_type = 3;
                    changeState(States::SEARCHING_TURNING);


                } else if(distance_ultrasonic_sensor <= 80) {
                    g_obstacle_type = 3;
                    changeState(States::SEARCHING_TURNING);

                } else if(levy_timer.read_us() > levy_target_time_us) {
                    g_back_count++;
                    set_new_levy_time_flag = 1;
                    piswarm.play_tune("G",1);
                    piswarm.set_oled_colour(255,0,0);
                    changeState(States::SEARCHING_TURNING);
                } else if(g_back_count >=8) {
                    g_back_count = 0;
                    piswarm.stop();
                    wait_ms(200);
                    piswarm.left_motor(-0.4*l_mot_scaler);
                    piswarm.right_motor(-0.4*r_mot_scaler);
                    wait(0.5);
                    changeState(States::SEARCHING_TURNING);

                } else if (distance_ultrasonic_sensor <= 800) {
                    consecutive_turn_count = 0;
                    //decrease speed when PiSwarm is getting close to an obstacle
                    float velocity = 0.2;
                    if(distance_ultrasonic_sensor > 100) {
                        velocity = (distance_ultrasonic_sensor*0.00057) + 0.142;
                    }
                    piswarm.left_motor(velocity*l_mot_scaler);
                    piswarm.right_motor(velocity*r_mot_scaler);
                    wait_ms(100);


                    //Otherwise continue moving forward until distance determined by levy algorithm is calculated.


                } else {
                    consecutive_turn_count = 0;
                    piswarm.right_motor(BASE_SPEED*r_mot_scaler);
                    piswarm.left_motor(BASE_SPEED*l_mot_scaler);
                }
                //wait to get new ultrasound reading
                //wait_ms(50);


            } else if(gv_state == States::SEARCHING_TURNING) {
                piswarm.stop();//Stop the robot.
                int16_t randomAngle;
                //If sent here beacuse of obstacle find angle between -180 to -90 and 90 to 180
                if(g_obstacle_type == 1) {
                    randomAngle = rand()%90 - 135;

                } else if(g_obstacle_type == 2) {
                    randomAngle = rand()%90 + 45;

                } else if(g_obstacle_type == 3) {
                    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
                g_obstacle_type = 0;
                changeState(States::SEARCHING_FWD);//Move back into state 11

            }
            //Beacon found state
        } else if (gv_state == States::MOVING_TO_BEACON) {

            //Do something here on receipt of 'function 5' if necessary.
            //As currently the home beacon will immediately switch on that is not necessary.

            static int16_t valid_distances[8] = {0};

            int16_t maxValue[2] = {0,100};   //Value and sensor position
            int8_t loopCounter = 0;

            if(beacon_illuminated_flag == 0) {
                for(loopCounter = 0; loopCounter < 8; loopCounter++) {
                    valid_distances[loopCounter] = gv_IRDistances[loopCounter];
                }
            }

            //If beacon visible
            if(beacon_syncronised_flag == 1) {

                //Firstly check beacon is still visible
                beacon_syncronised_flag = 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( valid_distances[loopCounter] > 100 && valid_distances[mod8((loopCounter + 1),8)]>100 && valid_distances[mod8((loopCounter - 1),8)]>100) {
                            if(gv_IRVals[g_beaconOn][loopCounter] > maxValue[0]) {
                                maxValue[0] = gv_IRVals[g_beaconOn][loopCounter];
                                maxValue[1] = loopCounter;
                                beacon_syncronised_flag = 1; //This will remain as one so long as at least on sensor can see beacon
                            }
                        }
                    }
                }

                //Only do this if beacon still visible
                if(beacon_syncronised_flag == 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
                        calculateNewHeading();

                        if(g_currentHeading > 5 || g_currentHeading < -5) {
                            turnDegrees(-g_currentHeading);
                        }

                        //If the beacon is not currently on but obstacle detected then do obstacle avoidance
                        int16_t randomAngle;
                        if(beacon_illuminated_flag == 0) {
                            if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100) {
                                back_move_timer.reset();
                                randomAngle = rand()%90 - 135;
                                piswarm.stop();
                                wait_ms(100);
                                piswarm.backward(0.3);
                                wait_ms(200);
                                turnDegrees(randomAngle);
                            } else if (gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100) {
                                back_move_timer.reset();
                                randomAngle = rand()%90 + 45;
                                piswarm.stop();
                                wait_ms(100);
                                piswarm.backward(0.3);
                                wait_ms(200);
                                turnDegrees(randomAngle);
                            } else if ( distance_ultrasonic_sensor < 100) {
                                back_move_timer.reset();
                                randomAngle = rand()%60 - 30;
                                piswarm.stop();
                                wait_ms(100);
                                piswarm.backward(0.3);
                                wait_ms(200);
                                turnDegrees(randomAngle);
                            } else if (back_move_timer.read() > 10){
                                back_move_timer.reset();
                                randomAngle = rand()%90 + 45;
                                piswarm.stop();
                                wait_ms(100);
                                piswarm.backward(0.5);
                                wait_ms(200);
                                turnDegrees(randomAngle);
                            }
                        }
                        piswarm.right_motor(0.3*r_mot_scaler*r_mot_scaler);
                        piswarm.left_motor(0.3*l_mot_scaler*l_mot_scaler);
                        wait_ms(500);
                        //Should be at beacon
                    } else {
                        piswarm.stop();
                        //homero comment this 
/*                        calculateNewHeading();
                        if(g_currentHeading > 5 || g_currentHeading < -5) {
                                turnDegrees(-g_currentHeading);*/

                        //If either of these flags is one then the beacon should be the home beacon so change to state 4.
                        if(return_flag == 1 || back_flag == 1) {
                            changeState(States::AT_HOME_BEACON);
                        } else {
                            changeState(States::AT_TARGET_BEACON);
                        }
                    }
                }
                //Else need to syncronise with beacon
            } else {

                while(beacon_syncronised_flag == 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_IRValDiffsTwo[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED) {
                            testAfter = 1;
                        }
                        if (gv_IRValDiffsTwo[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(States::SEARCHING_FWD);
                        beacon_syncronised_flag = 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 10ms
                    } else if(testBefore == 0 && testDuring == 1 && testAfter == 1) {
                        if(sync_attempt_count > 5){
                            changeState(States::SEARCHING_FWD);
                            beacon_syncronised_flag = 1;//to exit while loop
                        }
                        else
                        {
                            ticker_25ms.detach();
                            tickChangeTimeout.attach_us(&atTimeout,7000);
                            wait(1);//Do not delete this wait
                            sync_attempt_count++;
                        }

                        //If successful the set flag
                    } else if (testBefore == 0 && testDuring == 1 && testAfter == 2) {
                        beacon_syncronised_flag = 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();
                        ticker_25ms.detach();
                        tickChangeTimeout.attach_us(&atTimeout,10000);
                        wait_ms(500);
                        beacon_syncronised_flag = 1;
                        changeState(States::SEARCHING_FWD);
                    }
                }
            }

            //At target Beacon.
            //Broadcast target beacon found and wait.
        } else if (gv_state == States::AT_TARGET_BEACON) {
            
            int8_t aligned_to_beacon = 0;
            piswarm.stop();
            piswarm.set_oled_colour(150,255,0);
            piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
            
            //to fully aligned when its near to the target beacon 
            calculateNewHeading();
            while(aligned_to_beacon == 0){
                if(g_currentHeading > 5 || g_currentHeading < -5) {
                    wait(1);
                    turnDegrees(-g_currentHeading);
                }else{
                    aligned_to_beacon = 1;
                }
            }
            
            //Detach tickers before broadcasting and waiting for reply
            ticker_25ms.detach();
            ticker_ultrasonic50ms.detach();

            uint8_t const num_to_broadcast = 10;

            while(back_flag == 0) {
                if(broadcasted_flag == 1 && broadcasted_count < num_to_broadcast) {
                    broadcastTimeout.attach_us(&atBroadcastTimeout,100000);
                    broadcasted_flag = 0;
                }
            }

            ticker_25ms.attach_us(&readIRs,25000);
            ticker_ultrasonic50ms.attach_us(&get_ultrasonic_readings,50000);

            //Return to beacon search state but now robot is lookling for the home beacon.
            changeState(States::SEARCHING_FWD);

            //Back at home area. Stop and wait.
        } else if (gv_state == States::AT_HOME_BEACON) {
            piswarm.stop();
            piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
            piswarm.play_tune(  "T180L8O5EERERCL4EGR<GR",22  );
            while(1) {
                piswarm.set_oled_colour(0,150,0);
                wait_ms(200);
                piswarm.set_oled_colour(150,0,0);
                wait_ms(200);
                piswarm.set_oled_colour(0,0,150);
                wait_ms(200);
            }
        }
    }
}

/***************************************************************************************************************************************
 *
 * 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...
    piswarm.cls();
    piswarm.locate(0,0);
    piswarm.printf("URF:%d",function);
    if(length > 0) {
        piswarm.locate(0,1);
        piswarm.printf("%s",data);
    }

    if(function == 1) {
        start_flag = 1;
    }

    if(function == 5) {
        stopSearchTimeout.attach(&atStopSearchTimeout,6);
        return_flag = 1;
    }
    if(function == 6) {
        back_flag = 1;
    }
}

// This function is used to send the RF message:
void broadcast_user_rf_command(int function, char * message, int length)
{
    //This function augments the communications stack
    //It sends a 'user' RF command to all members (ie target_id = 0)
    //It sends a 'request', not a 'command', meaning it will still be handled if commands are disabled (RF_ALLOW_COMMANDS set to 0, recommended)
    //It takes three inputs:
    // * function (an integer from 0 to 15)
    // * message  (a char array)
    // * length   (length of message in bytes)
    send_rf_message(0,48+(function % 16),message,length);
}

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