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:
- 13:c18d82f62d38
- Parent:
- 12:118f2b0ed8eb
- Child:
- 14:fc406dfff94f
--- a/main.cpp Tue Aug 11 15:21:19 2015 +0000
+++ b/main.cpp Sun Aug 23 14:26:12 2015 +0000
@@ -33,19 +33,34 @@
* @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
-* @author Evans
+* @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;
@@ -53,27 +68,44 @@
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 = 50; //Value by which consecutive IR sensor readings need to jump by for in order to cause beacon to be suspected.
-uint16_t const AT_BEACON_THRESHOLD = 3700;
-uint8_t volatile gv_state = 0; //This is the current state of the finite state machine
+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.4;
+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 = 0;
+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;
-int8_t flagStationary = 1; //Used to determine if robot is currentl stationary or moving.
+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.
@@ -96,14 +128,18 @@
//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];
+ 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){
@@ -112,15 +148,19 @@
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){
@@ -128,69 +168,175 @@
}
//#put this into a function
//Switch case for robot 5
- switch(loopCounter){
- case 0:
- temp = 662 * sqrt(1/(temp-148));
- break;
- case 1:
- temp = 662 * sqrt(1/(temp-144));
- break;
- case 2:
- temp = 662 * sqrt(1/(temp-120));
- break;
- case 3:
- temp = 662 * sqrt(1/(temp-148));
+ 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 4:
- temp = 662 * sqrt(1/(temp-120));
- break;
- case 5:
- temp = 662 * sqrt(1/(temp-132));
+ 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;
- case 6:
- temp = 662 * sqrt(1/(temp-152));
- break;
- case 7:
- temp = 662 * sqrt(1/(temp-212));
- 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();
- ticker_25ms.attach_us(&readIRs,25000);
+ robot_id = piswarm.get_id();
//starting point in state 11
- timer.start();
- timerLevy.start();
- wait(1); //Wait a second to allow IR array to be filled
+ //usensor.start(); // get first reading of the ultrasonic sensor required before ticker
+ //wait_ms(50);
- //changeState(11);
+
+ //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 == 0){
+ 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
- changeState(11);
+ 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);
}
- wait(1);
-
-
+
//Searching state
- } else if (gv_state == 11 || gv_state == 12){
+ } 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.
@@ -198,7 +344,6 @@
//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){
- //piswarm.stop();
//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;
@@ -218,8 +363,8 @@
//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(2);
+ //wait(2);
+ changeState(States::MOVING_TO_BEACON);
} else {
//Reset the flag to try again
tick_beacon_period_check = 100;
@@ -227,40 +372,99 @@
}
}
- if(gv_state == 11){
+ 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);
+ //wait(0.1);
flagObstacle = 1;
- changeState(12);
+ 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);
+ //wait(0.1);
flagObstacle = 2;
- changeState(12);
-
- //Otherwise continue moving forward until distance determined by levy algorithm is calculated.
+ 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);
- changeState(12);
+ 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 if (flagStationary == 1){
-
- piswarm.forward(BASE_SPEED);
- flagStationary = 0;
- }
+ } 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 == 12){
+
+ } else if(gv_state == States::SEARCHING_TURNING){
piswarm.stop();//Stop the robot.
- flagStationary = 1; //update this flag
int16_t randomAngle;
//If sent here beacuse of obstacle find angle between -180 to -90 and 90 to 180
if(flagObstacle == 1){
@@ -269,32 +473,85 @@
} 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);
+ //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(11);//Move back into state 11
+ 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 == 2){
+ } 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);
- uint8_t loopCounter = 0;
+ 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){
@@ -306,11 +563,13 @@
//Find which sensor has the highest reading
if( gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED) {
- if(gv_IRVals[g_beaconOn][loopCounter] > maxValue[0]){
- maxValue[0] = gv_IRVals[g_beaconOn][loopCounter];
- maxValue[1] = loopCounter;
+ 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
+ }
}
- flagBeaconSyncronised = 1; //This will remain as one so long as at least on sensor can see beacon
}
}
@@ -349,37 +608,60 @@
//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){
+ //printf("%d ",g_currentHeading);
+ if(g_currentHeading > 5 || g_currentHeading < -5){
turnDegrees(-g_currentHeading);
}
- printf("%d\n",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){
- turnDegrees(-90);
+ 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){
- turnDegrees(90);
+ 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.forward(0.2);
- wait(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();
+ //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(4);
+ changeState(States::AT_HOME_BEACON);
} else {
- changeState(3);
+ changeState(States::AT_TARGET_BEACON);
}
}
}
@@ -399,29 +681,28 @@
if (gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED){
testDuring = 1;
}
- if (gv_IRValDiffs[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){
+ if (gv_IRValDiffsTwo[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){
testAfter = 1;
}
- if (gv_IRValDiffs[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] < -BEACON_SUSPECTED){
+ 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(11);
+ 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 15ms
+ //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();
- timer.reset();
- while(timer.read_ms() < 100){};
- ticker_25ms.attach_us(&readIRs,25000);
- wait(1);
+ //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){
@@ -434,48 +715,72 @@
piswarm.cls();
piswarm.printf("%d %d %d",testBefore, testDuring,testAfter);
piswarm.stop();
- wait(2);
- changeState(11);
+ 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 == 3){
-
+ } 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);
- //The value of function will change depending on the type of TAG used
- char function = 2;
- char message[2] = {"0"};
- broadcast_user_rf_command(function,message,1);
- wait(0.5);
- broadcast_user_rf_command(function,message,1);
- wait(0.5);
- broadcast_user_rf_command(function,message,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(0.5);
+ //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(11);
+ changeState(States::SEARCHING_FWD);
//Back at home area. Stop and wait.
- } else if (gv_state == 4){
+ } 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(0.2);
+ wait_ms(200);
piswarm.set_oled_colour(150,0,0);
- wait(0.2);
+ wait_ms(200);
piswarm.set_oled_colour(0,0,150);
- wait(0.2);
+ wait_ms(200);
}
}
}
@@ -567,4 +872,4 @@
char switches = piswarm.get_switches();
//Do something...
-}
+}
\ No newline at end of file