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:
- 17:4fa7c9e1b512
- Parent:
- 16:a32ee9ed15c4
--- a/main.cpp Sun Aug 23 17:22:54 2015 +0000
+++ b/main.cpp Tue Aug 25 17:49:16 2015 +0000
@@ -59,10 +59,12 @@
//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
@@ -77,6 +79,9 @@
//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
@@ -105,7 +110,7 @@
//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.
+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.
@@ -255,7 +260,7 @@
int static count = 0;
distance_ultrasonic_sensor = usensor.get_dist_mm();
if(count <100) {
- pc.printf("US: %.1f, %d\n",distance_ultrasonic_sensor, count);
+ //pc.printf("US: %.1f, %d\n",distance_ultrasonic_sensor, count);
count ++;
} else {
count =0;
@@ -264,8 +269,6 @@
if(distance_ultrasonic_sensor <= 0 || distance_ultrasonic_sensor == 1000) {
distance_ultrasonic_sensor = old_dist;
}
- //piswarm.cls();
-
usensor.start();
}
@@ -278,9 +281,9 @@
void atBroadcastTimeout()
{
char function;
- if(robot_id == 1|| robot_id == 12) {
+ if(robot_id == 1|| robot_id == 9) {
function = 2;
- } else if(robot_id == 9) {
+ } else if(robot_id == 12) {
function = 3;
}
@@ -290,6 +293,10 @@
broadcasted_count++;
broadcasted_flag = 1;
}
+
+void atStopSearchTimeout(){
+ stop_search_flag = 1;
+}
//This is a wait function for one
//*******************************************************************************************************
@@ -320,13 +327,13 @@
}
else if (robot_id == 9) {
- at_beacon_threshold = 3950;
+ at_beacon_threshold = 3925;
}
else if (robot_id == 12) {
- at_beacon_threshold = 3980;
- r_mot_scaler = 1.02;
- l_mot_scaler = 0.98;
+ at_beacon_threshold = 3825;
+ r_mot_scaler = 1.03;
+ l_mot_scaler = 0.97;
}
else {
@@ -382,8 +389,20 @@
if(gv_state == States::SEARCHING_FWD) {
-
- if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100) {
+ 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");
@@ -430,6 +449,7 @@
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) {
@@ -437,13 +457,14 @@
}
piswarm.left_motor(velocity*l_mot_scaler);
piswarm.right_motor(velocity*r_mot_scaler);
- //wait_ms(0.2);
+ 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);
}
@@ -553,6 +574,7 @@
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);
@@ -560,6 +582,7 @@
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);
@@ -567,12 +590,21 @@
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);
@@ -581,10 +613,10 @@
//Should be at beacon
} else {
piswarm.stop();
- calculateNewHeading();
+ //homero comment this
+/* calculateNewHeading();
if(g_currentHeading > 5 || g_currentHeading < -5) {
- turnDegrees(-g_currentHeading);
- }
+ 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) {
@@ -629,9 +661,17 @@
//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(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) {
@@ -656,11 +696,23 @@
//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();
@@ -737,6 +789,7 @@
}
if(function == 5) {
+ stopSearchTimeout.attach(&atStopSearchTimeout,6);
return_flag = 1;
}
if(function == 6) {