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
Diff: main.cpp
- Revision:
- 15:f7a8989a3cd3
- Parent:
- 14:fc406dfff94f
- Child:
- 16:a32ee9ed15c4
--- a/main.cpp Sun Aug 23 16:27:14 2015 +0000
+++ b/main.cpp Sun Aug 23 16:33:13 2015 +0000
@@ -39,7 +39,7 @@
* @author Homero Silva
* @date 16/08/15
* @ultrasonic sensor added
-* @version 3.0
+* @version 3.0
* @author Robert Evans
* @date 17/08/15
* @communication added
@@ -102,7 +102,7 @@
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
+//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 = 1; //Set to 1 on receiving a command by RF. Indicates robot can start searching for target beacon.
@@ -115,14 +115,15 @@
//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(){
-
+void readIRs()
+{
+
//Fistly update the beaconVisable flag if possible
- if(gv_counter25ms == mod8((g_beaconOn - 1), IR_READ_PER_BEAC)){
+ 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)){
+
+ if(gv_counter25ms == mod8((g_beaconOn + 2), IR_READ_PER_BEAC)) {
beacon_illuminated_flag = 0;
}
//Firstly store background values
@@ -137,49 +138,49 @@
//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){
+ 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
+
+ //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){
+ if(gv_counter25ms == 0) {
pc.printf("sen %d raw: %f",loopCounter,temp);
}
- if(temp>3500){
- temp = 3500;
- } else if (temp < 97){
- temp = 97;
+ if(temp>3500) {
+ temp = 3500;
+ } else if (temp < 97) {
+ temp = 97;
}
- //#put this into a function
+ //#put this into a function
//Switch case for robot 5
- switch(robot_id){
+ switch(robot_id) {
case 9:
- switch(loopCounter){
+ switch(loopCounter) {
case 0:
temp = 1643 * sqrt(1/(temp-190))-35;
- break;
+ break;
case 1:
temp = 1097 * sqrt(1/(temp-190))-24;
- break;
+ break;
case 2:
temp = 838 * sqrt(1/(temp-120))-17;
break;
@@ -201,13 +202,13 @@
}
break;
case 12:
- switch(loopCounter){
+ switch(loopCounter) {
case 0:
temp = 877 * sqrt(1/(temp-80))-13;
- break;
+ break;
case 1:
temp = 887 * sqrt(1/(temp-110))-13;
- break;
+ break;
case 2:
temp = 744 * sqrt(1/(temp-90))-8;
break;
@@ -229,57 +230,60 @@
}
break;
default:
- temp = 662 * sqrt(1/(temp-152));
+ temp = 662 * sqrt(1/(temp-152));
}
-
- if(gv_counter25ms == 0){
- pc.printf("sen %d dist:%f\n",loopCounter,temp);
+
+ if(gv_counter25ms == 0) {
+ pc.printf("sen %d dist:%f\n",loopCounter,temp);
}
- if (temp > 130){
+ if (temp > 130) {
temp = 130;
}
- gv_IRDistances[loopCounter] = temp;
+ 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(){
+//Get the distance of the ultrsonic sensor in cm
+void get_ultrasonic_readings()
+{
float old_dist = distance_ultrasonic_sensor;
- int static count = 0;
+ int static count = 0;
distance_ultrasonic_sensor = usensor.get_dist_mm();
- if(count <100){
+ if(count <100) {
pc.printf("US: %.1f, %d\n",distance_ultrasonic_sensor, count);
count ++;
- }else{
+ } else {
count =0;
}
-
- if(distance_ultrasonic_sensor <= 0 || distance_ultrasonic_sensor == 1000){
+
+ if(distance_ultrasonic_sensor <= 0 || distance_ultrasonic_sensor == 1000) {
distance_ultrasonic_sensor = old_dist;
}
- //piswarm.cls();
-
+ //piswarm.cls();
+
usensor.start();
}
//When called the readIRs ticker will be reattached after the specified time.
-void atTimeout(){
+void atTimeout()
+{
ticker_25ms.attach_us(&readIRs,25000);
}
-void atBroadcastTimeout(){
+void atBroadcastTimeout()
+{
char function;
- if(robot_id == 1|| robot_id == 12){
+ if(robot_id == 1|| robot_id == 12) {
function = 2;
- } else if(robot_id == 9){
+ } else if(robot_id == 9) {
function = 3;
}
-
+
char message[2];
message[0] = piswarm.get_id()+48;
broadcast_user_rf_command(function,message,1);
@@ -289,83 +293,84 @@
//This is a wait function for one
//*******************************************************************************************************
-//This is where the program code goes.
-int main() {
- init();
+//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
+ //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){
-
+ while(1) {
+
//Waiting for signal to begin searching
- if(gv_state == States::READY_TO_START){
+ if(gv_state == States::READY_TO_START) {
//pc.printf("%d\n",start_flag);
- if(start_flag == 1){
+ if(start_flag == 1) {
//Change state here after recieving a radio command
ticker_25ms.attach_us(&readIRs,25000);
-
- if(robot_id == 1){
+
+ if(robot_id == 1) {
at_beacon_threshold = 3850;
}
-
- else if (robot_id == 9){
+
+ else if (robot_id == 9) {
at_beacon_threshold = 3950;
}
-
- else if (robot_id == 12){
+
+ else if (robot_id == 12) {
at_beacon_threshold = 3980;
r_mot_scaler = 1.02;
l_mot_scaler = 0.98;
}
-
- else{
+
+ else {
at_beacon_threshold = 3900;
}
-
- usensor.start(); // get first reading of the ultrasonic sensor required before ticker
+
+ 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){
-
+
+ //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){
+ if(tick_beacon_suspected != 100) {
//When the beacon flag is first raised store its value and reset it
- if(tick_beacon_period_check == 100){
+ 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.
+ //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){
+ 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 {
@@ -374,337 +379,338 @@
}
}
}
-
- if(gv_state == States::SEARCHING_FWD){
-
-
- if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
+
+ if(gv_state == States::SEARCHING_FWD) {
+
+
+ if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100) {
piswarm.stop();
- piswarm.cls();
+ 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){
+
+ //Avoid obstacle to the left
+ } else if(gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100) {
piswarm.stop();
- piswarm.cls();
+ 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){
+ changeState(States::SEARCHING_TURNING);
+
+ } else if(distance_ultrasonic_sensor < 70) {
piswarm.stop();
- piswarm.cls();
+ 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){
+
+
+ } 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){
+ } 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);
+ 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 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);
+ piswarm.left_motor(BASE_SPEED*l_mot_scaler);
}
//wait to get new ultrasound reading
- //wait_ms(50);
-
-
- } else if(gv_state == States::SEARCHING_TURNING){
+ //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){
+ if(g_obstacle_type == 1) {
randomAngle = rand()%90 - 135;
-
- } else if(g_obstacle_type == 2){
+
+ } else if(g_obstacle_type == 2) {
randomAngle = rand()%90 + 45;
-
- } else if(g_obstacle_type == 3){
+
+ } else if(g_obstacle_type == 3) {
randomAngle = rand()%90 + 45;
-
- //Otherwise if here due to levy walk: turn to any random angle
+
+ //Otherwise if here due to levy walk: turn to any random angle
} else {
- randomAngle = rand()%360 - 180;
+ 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];
- }
- }
+
+ //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.
- //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
- }
- }
+ 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];
}
}
-
- //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 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++) {
- 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;
+ //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
+ }
}
}
}
-
-
- //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();
+
+ //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++) {
- 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){
- 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){
- randomAngle = rand()%90 + 45;
- piswarm.stop();
- wait_ms(100);
- piswarm.backward(0.3);
- wait_ms(200);
- turnDegrees(randomAngle);
- } else if ( distance_ultrasonic_sensor < 100){
- randomAngle = rand()%60 - 30;
- piswarm.stop();
- wait_ms(100);
- piswarm.backward(0.3);
- wait_ms(200);
- turnDegrees(randomAngle);
+ //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;
+ }
}
}
- 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();
- if(g_currentHeading > 5 || g_currentHeading < -5){
- turnDegrees(-g_currentHeading);
+
+
+ //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) {
+ 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) {
+ randomAngle = rand()%90 + 45;
+ piswarm.stop();
+ wait_ms(100);
+ piswarm.backward(0.3);
+ wait_ms(200);
+ turnDegrees(randomAngle);
+ } 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.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();
+ 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);
+ }
}
-
- //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 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) {
+ ticker_25ms.detach();
+ 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) {
+ beacon_syncronised_flag = 1;
+
+ //Error handle. If this happens stop the piswarm
} else {
- changeState(States::AT_TARGET_BEACON);
+ 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);
}
}
}
- //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){
- ticker_25ms.detach();
- 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){
- 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) {
+
+ 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(back_flag == 0) {
+ if(broadcasted_flag == 1 && broadcasted_count < num_to_broadcast) {
+ broadcastTimeout.attach_us(&atBroadcastTimeout,100000);
+ broadcasted_flag = 0;
}
}
- }
-
- //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(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);
}
}
-
- 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
+ * 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){
+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
@@ -714,28 +720,28 @@
// 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);
+ piswarm.printf("%s",data);
}
- if(function == 1){
+ if(function == 1) {
start_flag = 1;
}
- if(function == 5){
+ if(function == 5) {
return_flag = 1;
}
- if(function == 6){
+ 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)
@@ -750,7 +756,8 @@
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){
+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
@@ -761,20 +768,22 @@
// * data = Array containing extra data bytes
// length = Length of extra data bytes held (range 0 to 57)
- //Do something...
-}
+ //Do something...
+}
-void processRawRFData(char * rstring, char cCount){
+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() {
+void switch_pressed()
+{
//Switch(es) pressed {1 = Center 2 = Right 4 = Left 8 = Down 16 = Up}
char switches = piswarm.get_switches();
-
+
//Do something...
}
\ No newline at end of file