Homero Silva
/
PRGP_Pi_Swarm_ground_search_algorithm
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
Pi_Swarm_Blank
by 
James Hilder
main.cpp
- Committer:
- homero
- Date:
- 2015-08-23
- Revision:
- 13:c18d82f62d38
- Parent:
- 12:118f2b0ed8eb
- Child:
- 14:fc406dfff94f
File content as of revision 13:c18d82f62d38:
/*
* 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 "PiSwarmControllerFunctions.h"
#include "hcsr04.h"
PiSwarm piswarm;
Serial pc (USBTX,USBRX);
HCSR04 usensor(p30,p14);
//Tickers
Ticker ticker_25ms;
Ticker ticker_ultrasonic50ms;
//Timeouts
Timeout tickChangeTimeout;
Timeout broadcastTimeout;
//Timers
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};
int16_t volatile gv_IRValDiffsTwo[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 = 20; //Value by which consecutive IR sensor readings need to jump by for in order to cause beacon to be suspected.
uint16_t volatile AT_BEACON_THRESHOLD = 3800;
float volatile distance_ultrasonic_sensor = 1000;
float r_mot_scaler = 1;
float l_mot_scaler = 1;
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 volatile gv_IRDistances[8]; //Using the custom distance function the active ir readings are converted to distances and stored here every 25ms.
//homero: ultrasonic reading
uint8_t volatile gv_state = States::READY_TO_START; //This is the current state of the finite state machine
int16_t g_currentHeading = 0;
float BASE_SPEED = 0.6;
int8_t g_beaconOn = 100;
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 g_ultrasound_count = 0;
uint8_t broadcasted_count = 0;
uint8_t go_back_count = 0;
//Flags
int8_t volatile start_flag = 1;
int8_t volatile back_flag = 0;
int8_t volatile return_flag = 0;
//int8_t volatile aligned_target_beacon = 0; //homero
int8_t flagObstacle = 0;
uint8_t flagStationary = 1; //Used to determine if robot is currently 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
uint8_t broadcasted_flag = 1;
uint8_t robot_id;
//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;
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;
}
//piswarm.cls();
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 == 12){
function = 2;
} else if(robot_id == 9){
function = 3;
}
char message[2];
message[0] = piswarm.get_id()+48;
broadcast_user_rf_command(function,message,1);
broadcasted_count++;
broadcasted_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 = 3950;
}
else if (robot_id == 12){
AT_BEACON_THRESHOLD = 3980;
r_mot_scaler = 1.02;
l_mot_scaler = 0.98;
}
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);
timer.start();
timerLevy.start();
//pc.printf("why\n");
changeState(States::SEARCHING_FWD);
}
//Searching state
} else if (gv_state == States::SEARCHING_FWD || gv_state == States::SEARCHING_TURNING || gv_state == States::SEARCHING_REDUCED_SPEED){
//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){
//Secondly if obstacles are detected ahead then execute a random turn.
//pc.printf("Start\n");
//Homero: This is the obstacle avoidance part need to also turn if the ultrasound reading is high
//Avoid obstacle to the right
//if(distance_ultrasonic_sensor < 500 && distance_ultrasonic_sensor >= 100)
// {
// g_ultrasound_count++;
//
// } else {
// g_ultrasound_count = 0;
// }
//if (distance_ultrasonic_sensor < 500 && distance_ultrasonic_sensor >= 100 && g_ultrasound_count > 3){
// changeState(States::SEARCHING_REDUCED_SPEED);
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(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);
//wait(0.1);
flagObstacle = 2;
changeState(States::SEARCHING_TURNING);
} else if(distance_ultrasonic_sensor < 50){
piswarm.stop();
piswarm.cls();
piswarm.printf("ob F");
//piswarm.printf("F: %f",distance_ultrasonic_sensor);
piswarm.play_tune("DD",1);
//wait(0.1);
flagObstacle = 3;
changeState(States::SEARCHING_TURNING);
} else if(distance_ultrasonic_sensor <= 80){
flagObstacle = 3;
changeState(States::SEARCHING_TURNING);
} else if(timerLevy.read_us() > levy_target_time_us){
go_back_count++;
flagSetNewLevyTime = 1;
piswarm.play_tune("G",1);
piswarm.set_oled_colour(255,0,0);
changeState(States::SEARCHING_TURNING);
}else if(go_back_count >=10){
go_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){
//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(0.2);
//Otherwise continue moving forward until distance determined by levy algorithm is calculated.
} else {
piswarm.right_motor(BASE_SPEED*r_mot_scaler);
piswarm.left_motor(BASE_SPEED*l_mot_scaler);
//wait_ms(200);
//flagStationary = 0;
}
//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(flagObstacle == 1){
randomAngle = rand()%90 - 135;
} else if(flagObstacle == 2){
randomAngle = rand()%90 + 45;
} else if(flagObstacle == 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
//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(States::SEARCHING_FWD);//Move back into state 11
} else if (gv_state == States::SEARCHING_REDUCED_SPEED){
if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
piswarm.stop();
piswarm.cls();
piswarm.printf("ob R");
piswarm.play_tune("CC",1);
flagObstacle = 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);
flagObstacle = 2;
changeState(States::SEARCHING_TURNING);
} else if(timerLevy.read_us() > levy_target_time_us){
flagSetNewLevyTime = 1;
piswarm.play_tune("G",1);
changeState(States::SEARCHING_TURNING);
} else if(distance_ultrasonic_sensor > 800 || distance_ultrasonic_sensor == 0){
changeState(States::SEARCHING_FWD);
} else if(distance_ultrasonic_sensor <= 50){
flagObstacle = 3;
changeState(States::SEARCHING_TURNING);
}else {
//decrease speed when PiSwarm is getting close to an obstacle
float velocity = 0.2;
if(distance_ultrasonic_sensor > 100){
velocity = (distance_ultrasonic_sensor*0.00043) + 0.157;
}
piswarm.left_motor(velocity);
piswarm.right_motor(velocity);
//wait_ms(0.2);
}
}
//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
//wait(1);
int8_t loopCounter = 0;
if(flagBeaconIlluminated == 0){
for(loopCounter = 0; loopCounter < 8; loopCounter++){
valid_distances[loopCounter] = gv_IRDistances[loopCounter];
}
}
//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( 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;
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);
}
//If the beacon is not currently on but obstacle detected then do obstacle avoidance
int16_t randomAngle;
if(flagBeaconIlluminated == 0){
if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
randomAngle = rand()%90 - 135;
piswarm.stop();
wait_ms(100);
piswarm.backward(0.3);
wait_ms(200);
turnDegrees(randomAngle);
//piswarm.forward(0.2);
// wait_ms(500);
} else if (gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
randomAngle = rand()%90 + 45;
piswarm.stop();
wait_ms(100);
piswarm.backward(0.3);
wait_ms(200);
turnDegrees(randomAngle);
// piswarm.forward(0.2);
// wait_ms(500);
} else if ( distance_ultrasonic_sensor < 100){
randomAngle = rand()%60 - 30;
piswarm.stop();
wait_ms(100);
piswarm.backward(0.3);
wait_ms(200);
turnDegrees(randomAngle);
//piswarm.forward(0.2);
// wait_ms(500);
}
}
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();
calculateNewHeading();
//printf("%d ",g_currentHeading);
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(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_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);
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 10ms
} else if(testBefore == 0 && testDuring == 1 && testAfter == 1){
ticker_25ms.detach();
//This was 100000? I think it should be 10000
tickChangeTimeout.attach_us(&atTimeout,10000);
wait(1);//Do not delete this wait
//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();
ticker_25ms.detach();
tickChangeTimeout.attach_us(&atTimeout,10000);
wait_ms(500);
flagBeaconSyncronised = 1;
changeState(States::SEARCHING_FWD);
}
}
}
//At target Beacon.
//Broadcast target beacon found and wait.
} else if (gv_state == States::AT_TARGET_BEACON){
piswarm.stop();
piswarm.set_oled_colour(150,255,0);
piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
//Detach tickers before broadcasting and waiting for reply
ticker_25ms.detach();
ticker_ultrasonic50ms.detach();
uint8_t const num_to_broadcast = 10;
/* while(aligned_target_beacon ==0){
if(gv_IRVals[gv_counter25ms][7] < AT_BEACON_THRESHOLD && gv_IRVals[gv_counter25ms][0] < AT_BEACON_THRESHOLD ){
calculateNewHeading();
wait(0.025);
turnDegrees(-g_currentHeading);
}
else if (gv_IRVals[gv_counter25ms][7] > AT_BEACON_THRESHOLD && gv_IRVals[gv_counter25ms][0] > AT_BEACON_THRESHOLD) {
aligned_target_beacon = 1;
}
else{
piswarm.printf("stuck");
}
}*/
while(back_flag == 0){
if(broadcasted_flag == 1 && broadcasted_count < num_to_broadcast){
broadcastTimeout.attach_us(&atBroadcastTimeout,100000);
broadcasted_flag = 0;
}
//Wait for the back flag to change
//wait_us(1000);
}
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){
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...
}