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:
- 16:a32ee9ed15c4
- Parent:
- 15:f7a8989a3cd3
- Child:
- 17:4fa7c9e1b512
diff -r f7a8989a3cd3 -r a32ee9ed15c4 main.cpp
--- a/main.cpp Sun Aug 23 16:33:13 2015 +0000
+++ b/main.cpp Sun Aug 23 17:22:54 2015 +0000
@@ -472,230 +472,231 @@
g_obstacle_type = 0;
changeState(States::SEARCHING_FWD);//Move back into state 11
- //Beacon found state
- } else if (gv_state == States::MOVING_TO_BEACON) {
+ }
+ //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};
- //Do something here on receipt of 'function 5' if necessary.
- //As currently the home beacon will immediately switch on that is not necessary.
+ 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];
+ }
+ }
- static int16_t valid_distances[8] = {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++) {
- 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];
+ //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
+ }
+ }
}
}
- //If beacon visible
+ //Only do this if beacon still 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
+ //If the adjacent two sensors are above the threshold too then they can also be marked as illuminated
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
- }
+ //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;
}
}
}
- //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;
+ //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(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;
- }
- }
+ //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);
}
-
- //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 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);
}
}
- }
- //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;
- }
+ 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);
}
- //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
+ //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 {
- 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);
+ changeState(States::AT_TARGET_BEACON);
}
}
}
+ //Else need to syncronise with beacon
+ } else {
- //At target Beacon.
- //Broadcast target beacon found and wait.
- } else if (gv_state == States::AT_TARGET_BEACON) {
+ while(beacon_syncronised_flag == 0) {
- piswarm.stop();
- piswarm.set_oled_colour(150,255,0);
- piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
+ //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;
+ }
+ }
- //Detach tickers before broadcasting and waiting for reply
- ticker_25ms.detach();
- ticker_ultrasonic50ms.detach();
+ //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
- uint8_t const num_to_broadcast = 10;
+ //If successful the set flag
+ } else if (testBefore == 0 && testDuring == 1 && testAfter == 2) {
+ beacon_syncronised_flag = 1;
- while(back_flag == 0) {
- if(broadcasted_flag == 1 && broadcasted_count < num_to_broadcast) {
- broadcastTimeout.attach_us(&atBroadcastTimeout,100000);
- broadcasted_flag = 0;
+ //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);
}
}
+ }
- ticker_25ms.attach_us(&readIRs,25000);
- ticker_ultrasonic50ms.attach_us(&get_ultrasonic_readings,50000);
+ //At target Beacon.
+ //Broadcast target beacon found and wait.
+ } else if (gv_state == States::AT_TARGET_BEACON) {
- //Return to beacon search state but now robot is lookling for the home beacon.
- changeState(States::SEARCHING_FWD);
+ 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;
- //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);
+ 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