Homero Silva / Mbed 2 deprecated 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.

Dependencies:   mbed

Fork of Pi_Swarm_Blank by James Hilder

main.cpp@10:da62735d6df9, 2015-07-24 (annotated)

Committer:
re633
Date:
Fri Jul 24 10:37:58 2015 +0000
Revision:
10:da62735d6df9
Parent:
9:ef0907fda2f1
Child:
11:c5094a68283f
Working search controller with code added to start pi swarms moving using RF. Use accompany RF modulecode 'PRGP_RF_MODULE'

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jah128 2:e806b595f9ce 1 /*******************************************************************************************
jah128 2:e806b595f9ce 2 *
jah128 2:e806b595f9ce 3 * University of York Robot Lab Pi Swarm Robot Library
jah128 1:37502eb3b70f 4 *
jah128 4:823174be9a6b 5 * "Blank" Program
jah128 4:823174be9a6b 6 *
jah128 4:823174be9a6b 7 * Use this file as the template to produce custom controllers
jah128 4:823174be9a6b 8 *
jah128 1:37502eb3b70f 9 * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
jah128 1:37502eb3b70f 10 *
jah128 4:823174be9a6b 11 * Version 0.6 February 2014
jah128 1:37502eb3b70f 12 *
jah128 2:e806b595f9ce 13 * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
jah128 2:e806b595f9ce 14 *
jah128 2:e806b595f9ce 15 ******************************************************************************************/
jah128 0:46cd1498a39a 16
jah128 3:1aa1de26966a 17 #include "main.h" // Certain parameters can be set by changing the defines in piswarm.h
re633 9:ef0907fda2f1 18 #include "PiSwarmControllerFunctions.h"
re633 9:ef0907fda2f1 19
jah128 0:46cd1498a39a 20
jah128 1:37502eb3b70f 21 PiSwarm piswarm;
jah128 1:37502eb3b70f 22 Serial pc (USBTX,USBRX);
jah128 0:46cd1498a39a 23
re633 9:ef0907fda2f1 24 //Tickers
re633 9:ef0907fda2f1 25 Ticker ticker_25ms;
re633 9:ef0907fda2f1 26 Timer timer;
re633 10:da62735d6df9 27 Timer timerLevy;
re633 9:ef0907fda2f1 28
re633 9:ef0907fda2f1 29 //Global Variables
re633 9:ef0907fda2f1 30 uint8_t const IR_READ_PER_BEAC = 20; //The number of IR readings between beacon flashes
re633 9:ef0907fda2f1 31 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
re633 9:ef0907fda2f1 32 int16_t volatile gv_IRValDiffs[IR_READ_PER_BEAC][8] = {0};
re633 9:ef0907fda2f1 33 uint8_t volatile beacon_detected[8] = {0};
re633 9:ef0907fda2f1 34 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)
re633 9:ef0907fda2f1 35 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.
re633 9:ef0907fda2f1 36 uint16_t const AT_BEACON_THRESHOLD = 3700;
re633 9:ef0907fda2f1 37 uint8_t volatile gv_state = 0; //This is the current state of the finite state machine
re633 9:ef0907fda2f1 38 uint32_t levyTargetTimeus = 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.
re633 9:ef0907fda2f1 39 uint8_t volatile gv_IRDistances[8]; //Using the custom distance function the active ir readings are converted to distances and stored here every 25ms.
re633 9:ef0907fda2f1 40 int16_t g_currentHeading = 0;
re633 9:ef0907fda2f1 41 float BASESPEED = 0.4;
re633 9:ef0907fda2f1 42 int8_t g_beaconOn = 100;
re633 9:ef0907fda2f1 43 int8_t volatile tickBeaconSuspected = 100; //Is set to the tick value within the period between beacon flashes that the beacon flash is suspected to begin at
re633 9:ef0907fda2f1 44 int8_t tickBeaconPeriodCheck = 100; //Is used to temporarily store the value of tickBeaconSuspected
re633 10:da62735d6df9 45 uint16_t const TURN_BACK_TIME = (800/BASESPEED); //Time between trying to continiue on path it wason before obstacle given by: (Distance in mm) / (robots speed)
re633 9:ef0907fda2f1 46 //Flags
re633 10:da62735d6df9 47 int8_t volatile flagSystemState = 0;
re633 9:ef0907fda2f1 48 int8_t flagObstacle = 0;
re633 9:ef0907fda2f1 49 int8_t flagStationary = 1; //Used to determine if robot is currentl stationary or moving.
re633 9:ef0907fda2f1 50 uint8_t flagBeaconSyncronised = 0; //Set to one when the robot is synchronised with the beacon
re633 9:ef0907fda2f1 51 uint8_t volatile flagBeaconIlluminated = 0; //Should be 1 when beacon is illuminated otherwise 0
re633 10:da62735d6df9 52 uint8_t flagSetNewLevyTime = 1; //Must start as 1
re633 9:ef0907fda2f1 53 //Ticker Function*************************************************************************************
re633 9:ef0907fda2f1 54 //This function is called by a ticker every 25ms
re633 9:ef0907fda2f1 55 //The function firstly stores the background IRS values.
re633 9:ef0907fda2f1 56 //Secondly if beacon is off it uses illuminated IR sensors to estimate distances
re633 9:ef0907fda2f1 57 //Each second it will update when it believes beacon illumination time to be
re633 9:ef0907fda2f1 58 //It will work out which sensors spotted then beacon and will illuminate the Leds accordingly
re633 9:ef0907fda2f1 59 void readIRs(){
re633 9:ef0907fda2f1 60
re633 9:ef0907fda2f1 61 //Fistly update the beaconVisable flag if possible
re633 9:ef0907fda2f1 62 if(gv_counter25ms == mod8((g_beaconOn - 1), IR_READ_PER_BEAC)){
re633 9:ef0907fda2f1 63 flagBeaconIlluminated = 1;
re633 9:ef0907fda2f1 64 }
re633 9:ef0907fda2f1 65
re633 9:ef0907fda2f1 66 if(gv_counter25ms == mod8((g_beaconOn + 2), IR_READ_PER_BEAC)){
re633 9:ef0907fda2f1 67 flagBeaconIlluminated = 0;
re633 9:ef0907fda2f1 68 }
re633 9:ef0907fda2f1 69 //Firstly store background values
re633 9:ef0907fda2f1 70 //Also make a note of which point in the second did the values change most.
re633 9:ef0907fda2f1 71 //For which sensor specifically did the values change the most
re633 9:ef0907fda2f1 72 //That sensor will be used to estimate the beacon start time if a threshold value is met
re633 9:ef0907fda2f1 73 piswarm.store_background_raw_ir_values();
re633 9:ef0907fda2f1 74 int16_t IRchange = 0;
re633 9:ef0907fda2f1 75 uint8_t loopCounter = 0;
re633 9:ef0907fda2f1 76 //In this loop the raw IR values are read.
re633 9:ef0907fda2f1 77 //If the points where the IR values have increased by the greatest amount are noted as this indicates a beacon illumination
re633 9:ef0907fda2f1 78 for(loopCounter = 0; loopCounter < 8; loopCounter++) {
re633 9:ef0907fda2f1 79 gv_IRVals[gv_counter25ms][loopCounter] = piswarm.get_background_raw_ir_value(loopCounter);
re633 9:ef0907fda2f1 80 IRchange = gv_IRVals[gv_counter25ms][loopCounter]-gv_IRVals[mod8((gv_counter25ms-1),IR_READ_PER_BEAC)][loopCounter];
re633 9:ef0907fda2f1 81
re633 9:ef0907fda2f1 82 gv_IRValDiffs[gv_counter25ms][loopCounter] = IRchange;
re633 9:ef0907fda2f1 83 //printf("change %d count %d\n",IRchange);
re633 9:ef0907fda2f1 84 //If difference is greater than a threshold value then the beacon is suspected. This will be confirmed depending on the robots state of movement.
re633 9:ef0907fda2f1 85 if (IRchange > BEACON_SUSPECTED){
re633 9:ef0907fda2f1 86 tickBeaconSuspected = gv_counter25ms;
re633 9:ef0907fda2f1 87 piswarm.cls();
re633 9:ef0907fda2f1 88 piswarm.printf("%d",tickBeaconSuspected);
re633 9:ef0907fda2f1 89 }
re633 9:ef0907fda2f1 90 }
re633 9:ef0907fda2f1 91
re633 9:ef0907fda2f1 92 //Now store the illuminated values if the beacon is not illuminated-
re633 9:ef0907fda2f1 93 piswarm.store_illuminated_raw_ir_values();
re633 9:ef0907fda2f1 94
re633 9:ef0907fda2f1 95 //In this loop convert each raw active IR reading into a distance estimate
re633 9:ef0907fda2f1 96 for(loopCounter = 0; loopCounter < 8; loopCounter++) {
re633 9:ef0907fda2f1 97
re633 9:ef0907fda2f1 98 //Specific sensor readings converted to distances
re633 9:ef0907fda2f1 99 float temp = piswarm.get_illuminated_raw_ir_value(loopCounter);
re633 9:ef0907fda2f1 100 if(temp>3500){
re633 9:ef0907fda2f1 101 temp = 3500;
re633 9:ef0907fda2f1 102 } else if (temp < 97){
re633 9:ef0907fda2f1 103 temp = 97;
re633 9:ef0907fda2f1 104 }
re633 9:ef0907fda2f1 105 //#put this into a function
re633 9:ef0907fda2f1 106 //Switch case for robot 5
re633 9:ef0907fda2f1 107 switch(loopCounter){
re633 9:ef0907fda2f1 108 case 0:
re633 9:ef0907fda2f1 109 temp = 662 * sqrt(1/(temp-148));
re633 9:ef0907fda2f1 110 break;
re633 9:ef0907fda2f1 111 case 1:
re633 9:ef0907fda2f1 112 temp = 662 * sqrt(1/(temp-144));
re633 9:ef0907fda2f1 113 break;
re633 9:ef0907fda2f1 114 case 2:
re633 9:ef0907fda2f1 115 temp = 662 * sqrt(1/(temp-120));
re633 9:ef0907fda2f1 116 break;
re633 9:ef0907fda2f1 117 case 3:
re633 9:ef0907fda2f1 118 temp = 662 * sqrt(1/(temp-148));
re633 9:ef0907fda2f1 119 break;
re633 9:ef0907fda2f1 120 case 4:
re633 9:ef0907fda2f1 121 temp = 662 * sqrt(1/(temp-120));
re633 9:ef0907fda2f1 122 break;
re633 9:ef0907fda2f1 123 case 5:
re633 9:ef0907fda2f1 124 temp = 662 * sqrt(1/(temp-132));
re633 9:ef0907fda2f1 125 break;
re633 9:ef0907fda2f1 126 case 6:
re633 9:ef0907fda2f1 127 temp = 662 * sqrt(1/(temp-152));
re633 9:ef0907fda2f1 128 break;
re633 9:ef0907fda2f1 129 case 7:
re633 9:ef0907fda2f1 130 temp = 662 * sqrt(1/(temp-212));
re633 9:ef0907fda2f1 131 break;
re633 9:ef0907fda2f1 132 }
re633 9:ef0907fda2f1 133
re633 9:ef0907fda2f1 134 if (temp > 130){
re633 9:ef0907fda2f1 135 temp = 130;
re633 9:ef0907fda2f1 136 }
re633 9:ef0907fda2f1 137 gv_IRDistances[loopCounter] = temp;
re633 9:ef0907fda2f1 138
re633 9:ef0907fda2f1 139 }
re633 9:ef0907fda2f1 140 //reset counter after 1 second (beacon period)
re633 9:ef0907fda2f1 141 gv_counter25ms = mod8(gv_counter25ms + 1,IR_READ_PER_BEAC);
re633 9:ef0907fda2f1 142 }
re633 9:ef0907fda2f1 143
re633 9:ef0907fda2f1 144 //*******************************************************************************************************
jah128 8:a789ef4fde52 145 //This is where the program code goes.
jah128 1:37502eb3b70f 146 int main() {
re633 9:ef0907fda2f1 147 init();
re633 9:ef0907fda2f1 148 ticker_25ms.attach_us(&readIRs,25000);
re633 9:ef0907fda2f1 149 //starting point in state 11
re633 9:ef0907fda2f1 150 timer.start();
re633 10:da62735d6df9 151 timerLevy.start();
re633 9:ef0907fda2f1 152 wait(1); //Wait a second to allow IR array to be filled
re633 9:ef0907fda2f1 153
re633 10:da62735d6df9 154 //changeState(11);
jah128 8:a789ef4fde52 155
re633 9:ef0907fda2f1 156 //Controller is a finite state machine
re633 9:ef0907fda2f1 157 while(1){
re633 9:ef0907fda2f1 158
re633 9:ef0907fda2f1 159 //Waiting for signal to begin searching
re633 9:ef0907fda2f1 160 if(gv_state == 0){
re633 10:da62735d6df9 161 if(flagSystemState == 1){
re633 10:da62735d6df9 162 changeState(11);
re633 10:da62735d6df9 163 }
re633 9:ef0907fda2f1 164 wait(1);
re633 9:ef0907fda2f1 165
re633 9:ef0907fda2f1 166 //Change state here after recieving a radio command
re633 9:ef0907fda2f1 167 //Searching state
re633 9:ef0907fda2f1 168 } else if (gv_state == 11 || gv_state == 12){
re633 9:ef0907fda2f1 169
re633 9:ef0907fda2f1 170 //Firstly determine if suspected beacon is actually the beacon.
re633 9:ef0907fda2f1 171 //This is done by checking the period between flashes matches the beacon period
re633 9:ef0907fda2f1 172 if(tickBeaconSuspected != 100){
re633 9:ef0907fda2f1 173 //piswarm.stop();
re633 9:ef0907fda2f1 174 //When the beacon flag is first raised store its value and reset it
re633 9:ef0907fda2f1 175 if(tickBeaconPeriodCheck == 100){
re633 9:ef0907fda2f1 176 tickBeaconPeriodCheck = tickBeaconSuspected;
re633 9:ef0907fda2f1 177 tickBeaconSuspected = 100;
re633 9:ef0907fda2f1 178 //Check the timing of the latest jump with the last one to see if period matches the Beacon.
re633 9:ef0907fda2f1 179 } else {
re633 9:ef0907fda2f1 180 piswarm.locate(0,1);
re633 9:ef0907fda2f1 181 piswarm.printf("%d %d",tickBeaconPeriodCheck,tickBeaconSuspected);
re633 9:ef0907fda2f1 182 //printf("%d %d *********************************",tickBeaconPeriodCheck,tickBeaconSuspected);
re633 9:ef0907fda2f1 183 //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.
re633 9:ef0907fda2f1 184 int8_t test = (tickBeaconPeriodCheck - tickBeaconSuspected);
re633 9:ef0907fda2f1 185
re633 9:ef0907fda2f1 186 test = test * test;
re633 9:ef0907fda2f1 187
re633 9:ef0907fda2f1 188 //if test is low then identify the beacon as the cause of the flags
re633 9:ef0907fda2f1 189 if(test < 2){
re633 9:ef0907fda2f1 190 //Beacon found change to state 2
re633 9:ef0907fda2f1 191 g_beaconOn = tickBeaconPeriodCheck; //update the global variable that stores when beacon flashes occur
re633 9:ef0907fda2f1 192
re633 9:ef0907fda2f1 193 wait(2);
re633 9:ef0907fda2f1 194 changeState(2);
re633 9:ef0907fda2f1 195 } else {
re633 9:ef0907fda2f1 196 //Reset the flag to try again
re633 9:ef0907fda2f1 197 tickBeaconPeriodCheck = 100;
re633 9:ef0907fda2f1 198 }
re633 9:ef0907fda2f1 199 }
re633 9:ef0907fda2f1 200 }
re633 9:ef0907fda2f1 201
re633 9:ef0907fda2f1 202 if(gv_state == 11){
re633 9:ef0907fda2f1 203 //Secondly if obstacles are detected ahead then execute a random turn.
re633 9:ef0907fda2f1 204 if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
re633 9:ef0907fda2f1 205 piswarm.stop();
re633 10:da62735d6df9 206 piswarm.cls();
re633 10:da62735d6df9 207 piswarm.printf("ob R");
re633 10:da62735d6df9 208 piswarm.play_tune("CC",1);
re633 9:ef0907fda2f1 209 wait(0.1);
re633 9:ef0907fda2f1 210 flagObstacle = 1;
re633 9:ef0907fda2f1 211 changeState(12);
re633 9:ef0907fda2f1 212 } else if(gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
re633 9:ef0907fda2f1 213 piswarm.stop();
re633 10:da62735d6df9 214 piswarm.cls();
re633 10:da62735d6df9 215 piswarm.printf("ob L");
re633 10:da62735d6df9 216 piswarm.play_tune("CC",1);
re633 9:ef0907fda2f1 217 wait(0.1);
re633 9:ef0907fda2f1 218 flagObstacle = 2;
re633 9:ef0907fda2f1 219 changeState(12);
re633 10:da62735d6df9 220
re633 10:da62735d6df9 221 //Otherwise continue moving forward until distance determined by levy algorithm is calculated.
re633 10:da62735d6df9 222 } else if(timerLevy.read_us() > levyTargetTimeus){
re633 10:da62735d6df9 223 flagSetNewLevyTime = 1;
re633 10:da62735d6df9 224 piswarm.play_tune("G",1);
re633 10:da62735d6df9 225 changeState(12);
re633 9:ef0907fda2f1 226
re633 9:ef0907fda2f1 227 } else if (flagStationary == 1){
re633 9:ef0907fda2f1 228
re633 9:ef0907fda2f1 229 piswarm.forward(BASESPEED);
re633 9:ef0907fda2f1 230 flagStationary = 0;
re633 9:ef0907fda2f1 231 }
re633 9:ef0907fda2f1 232
re633 9:ef0907fda2f1 233 } else if(gv_state == 12){
re633 9:ef0907fda2f1 234 piswarm.stop();//Stop the robot.
re633 9:ef0907fda2f1 235 flagStationary = 1; //update this flag
re633 9:ef0907fda2f1 236 int16_t randomAngle;
re633 9:ef0907fda2f1 237 //If sent here beacuse of obstacle find angle between -180 to -90 and 90 to 180
re633 9:ef0907fda2f1 238 if(flagObstacle == 1){
re633 10:da62735d6df9 239 randomAngle = rand()%90 - 135;
re633 9:ef0907fda2f1 240
re633 9:ef0907fda2f1 241 } else if(flagObstacle == 2){
re633 10:da62735d6df9 242 randomAngle = rand()%90 + 45;
re633 10:da62735d6df9 243
re633 10:da62735d6df9 244 //Otherwise if here due to levy walk: turn to any random angle
re633 9:ef0907fda2f1 245 } else {
re633 10:da62735d6df9 246 randomAngle = rand()%360 - 180;
re633 9:ef0907fda2f1 247 }
re633 9:ef0907fda2f1 248 turnDegrees(randomAngle); //Make the turn
re633 10:da62735d6df9 249 wait(0.1);
re633 10:da62735d6df9 250 /* if(gv_IRDistances[0] < 70 || gv_IRDistances[1] < 70 || gv_IRDistances[6] < 70 || gv_IRDistances[7] < 70){
re633 10:da62735d6df9 251 //do nothing. Aim is that robot will keep turning the same way until clear of obstacle
re633 10:da62735d6df9 252 //could put a count in here to doemergency escape manouvre if necessary
re633 10:da62735d6df9 253 piswarm.play_tune("CC",1);
re633 10:da62735d6df9 254 } else { */
re633 10:da62735d6df9 255 flagObstacle = 0;
re633 10:da62735d6df9 256 changeState(11);//Move back into state 11
re633 10:da62735d6df9 257
re633 9:ef0907fda2f1 258 }
re633 9:ef0907fda2f1 259
re633 9:ef0907fda2f1 260
re633 9:ef0907fda2f1 261 //Beacon found state
re633 9:ef0907fda2f1 262 } else if (gv_state == 2){
re633 9:ef0907fda2f1 263 int16_t maxValue[2] = {0,100}; //Value and sensor position
re633 9:ef0907fda2f1 264 //wait(1);
re633 9:ef0907fda2f1 265 uint8_t loopCounter = 0;
re633 9:ef0907fda2f1 266
re633 9:ef0907fda2f1 267 //If beacon visible
re633 9:ef0907fda2f1 268 if(flagBeaconSyncronised == 1){
re633 9:ef0907fda2f1 269
re633 9:ef0907fda2f1 270 //Firstly check beacon is still visible
re633 9:ef0907fda2f1 271 flagBeaconSyncronised = 0;
re633 9:ef0907fda2f1 272 //Update array concerning which IRs can see the beacon
re633 9:ef0907fda2f1 273 for(loopCounter = 0; loopCounter<8; loopCounter++) {
re633 9:ef0907fda2f1 274
re633 9:ef0907fda2f1 275 //Find which sensor has the highest reading
re633 9:ef0907fda2f1 276 if( gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED) {
re633 9:ef0907fda2f1 277 if(gv_IRVals[g_beaconOn][loopCounter] > maxValue[0]){
re633 9:ef0907fda2f1 278 maxValue[0] = gv_IRVals[g_beaconOn][loopCounter];
re633 9:ef0907fda2f1 279 maxValue[1] = loopCounter;
re633 9:ef0907fda2f1 280 }
re633 9:ef0907fda2f1 281 flagBeaconSyncronised = 1; //This will remain as one so long as at least on sensor can see beacon
re633 9:ef0907fda2f1 282 }
re633 9:ef0907fda2f1 283 }
re633 9:ef0907fda2f1 284
re633 9:ef0907fda2f1 285 //Only do this if beacon still visible
re633 9:ef0907fda2f1 286 if(flagBeaconSyncronised == 1){
re633 9:ef0907fda2f1 287
re633 9:ef0907fda2f1 288 //If the adjacent two sensors are above the threshold too then they can also be marked as illuminated
re633 9:ef0907fda2f1 289 for(loopCounter = 0; loopCounter<8; loopCounter++){
re633 9:ef0907fda2f1 290
re633 9:ef0907fda2f1 291 //reset all beacon detected values
re633 9:ef0907fda2f1 292 beacon_detected[loopCounter] = 0;
re633 9:ef0907fda2f1 293
re633 9:ef0907fda2f1 294 if(abs(maxValue[1] - loopCounter)< 3 || abs(maxValue[1] + 8 - loopCounter)< 3 || abs(maxValue[1] - 8 - loopCounter)< 3) {
re633 9:ef0907fda2f1 295 if(gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED){
re633 9:ef0907fda2f1 296 beacon_detected[loopCounter] = 1;
re633 9:ef0907fda2f1 297 }
re633 9:ef0907fda2f1 298 }
re633 9:ef0907fda2f1 299 }
re633 9:ef0907fda2f1 300
re633 9:ef0907fda2f1 301
re633 9:ef0907fda2f1 302 //Update the piswarm LEDS so the ones that can see the beacon are on.
re633 9:ef0907fda2f1 303 piswarm.set_oleds(beacon_detected[0]||beacon_detected[1],
re633 9:ef0907fda2f1 304 beacon_detected[1]||beacon_detected[2],
re633 9:ef0907fda2f1 305 beacon_detected[2],
re633 9:ef0907fda2f1 306 beacon_detected[3],
re633 9:ef0907fda2f1 307 0,
re633 9:ef0907fda2f1 308 beacon_detected[4],
re633 9:ef0907fda2f1 309 beacon_detected[5],
re633 9:ef0907fda2f1 310 beacon_detected[5]||beacon_detected[6],
re633 9:ef0907fda2f1 311 beacon_detected[6]||beacon_detected[7],
re633 9:ef0907fda2f1 312 beacon_detected[7]||beacon_detected[0]);
re633 9:ef0907fda2f1 313
re633 9:ef0907fda2f1 314 //If the max IR value is below a threshold then move toward beacon. Else change state
re633 9:ef0907fda2f1 315 if(maxValue[0] < AT_BEACON_THRESHOLD){
re633 9:ef0907fda2f1 316
re633 9:ef0907fda2f1 317 //Calculate the heading of Pi-Swarm Relative to beacon
re633 9:ef0907fda2f1 318 //printf("%d ",g_currentHeading);
re633 9:ef0907fda2f1 319 calculateNewHeading();
re633 9:ef0907fda2f1 320 printf("%d ",g_currentHeading);
re633 9:ef0907fda2f1 321 if(g_currentHeading > 5 ||g_currentHeading < -5){
re633 9:ef0907fda2f1 322 turnDegrees(-g_currentHeading);
re633 9:ef0907fda2f1 323 }
re633 9:ef0907fda2f1 324 printf("%d\n",g_currentHeading);
re633 9:ef0907fda2f1 325 //}
re633 9:ef0907fda2f1 326
re633 9:ef0907fda2f1 327
re633 9:ef0907fda2f1 328
re633 9:ef0907fda2f1 329 //If the beacon is not currently on but obstacle detected then do obstacle avoidance
re633 9:ef0907fda2f1 330
re633 9:ef0907fda2f1 331 if(flagBeaconIlluminated == 0){
re633 9:ef0907fda2f1 332 if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
re633 9:ef0907fda2f1 333 turnDegrees(90);
re633 9:ef0907fda2f1 334 } else if (gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
re633 9:ef0907fda2f1 335 turnDegrees(-90);
re633 9:ef0907fda2f1 336 }
re633 9:ef0907fda2f1 337 }
re633 9:ef0907fda2f1 338
re633 9:ef0907fda2f1 339
re633 9:ef0907fda2f1 340 piswarm.forward(0.2);
re633 9:ef0907fda2f1 341 wait(1);
re633 9:ef0907fda2f1 342 //Should be at beacon
re633 9:ef0907fda2f1 343 } else {
re633 9:ef0907fda2f1 344 piswarm.stop();
re633 9:ef0907fda2f1 345 changeState(3);
re633 9:ef0907fda2f1 346
re633 9:ef0907fda2f1 347 }
re633 9:ef0907fda2f1 348 }
re633 9:ef0907fda2f1 349 //Else need to syncronise with beacon
re633 9:ef0907fda2f1 350 } else {
re633 9:ef0907fda2f1 351
re633 9:ef0907fda2f1 352 while(flagBeaconSyncronised == 0){
re633 9:ef0907fda2f1 353 //Sychronise the ticker with the beacon
re633 9:ef0907fda2f1 354
re633 9:ef0907fda2f1 355 uint8_t testBefore = 0;
re633 9:ef0907fda2f1 356 uint8_t testDuring = 0;
re633 9:ef0907fda2f1 357 uint8_t testAfter = 0;
re633 9:ef0907fda2f1 358 for(loopCounter = 0; loopCounter < 8; loopCounter++){
re633 9:ef0907fda2f1 359 if (gv_IRValDiffs[mod8((g_beaconOn - 1),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){
re633 9:ef0907fda2f1 360 testBefore = 1;
re633 9:ef0907fda2f1 361 }
re633 9:ef0907fda2f1 362 if (gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED){
re633 9:ef0907fda2f1 363 testDuring = 1;
re633 9:ef0907fda2f1 364 }
re633 9:ef0907fda2f1 365 if (gv_IRValDiffs[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){
re633 9:ef0907fda2f1 366 testAfter = 1;
re633 9:ef0907fda2f1 367 }
re633 9:ef0907fda2f1 368 if (gv_IRValDiffs[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] < -BEACON_SUSPECTED){
re633 9:ef0907fda2f1 369 testAfter = 2;
re633 9:ef0907fda2f1 370 }
re633 9:ef0907fda2f1 371 }
re633 9:ef0907fda2f1 372 //Firstly if the beacon is not detected by any of the sensors then change state back to search
re633 9:ef0907fda2f1 373 if(testBefore == 0 && testDuring == 0 && testAfter == 0){
re633 9:ef0907fda2f1 374 changeState(11);
re633 9:ef0907fda2f1 375 flagBeaconSyncronised = 1;//to exit while loop
re633 9:ef0907fda2f1 376
re633 9:ef0907fda2f1 377 //If the tick before g_beaconOn is detecting the change caused by the flash change the value of g_beaconOn
re633 9:ef0907fda2f1 378 } else if(testBefore == 1){
re633 9:ef0907fda2f1 379 g_beaconOn = g_beaconOn - 1;
re633 9:ef0907fda2f1 380
re633 9:ef0907fda2f1 381 //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
re633 9:ef0907fda2f1 382 } else if(testBefore == 0 && testDuring == 1 && testAfter == 1){
re633 9:ef0907fda2f1 383 ticker_25ms.detach();
re633 9:ef0907fda2f1 384 timer.reset();
re633 9:ef0907fda2f1 385 while(timer.read_ms() < 100){};
re633 9:ef0907fda2f1 386 ticker_25ms.attach_us(&readIRs,25000);
re633 9:ef0907fda2f1 387 wait(1);
re633 9:ef0907fda2f1 388
re633 9:ef0907fda2f1 389 //If successful the set flag
re633 9:ef0907fda2f1 390 } else if (testBefore == 0 && testDuring == 1 && testAfter == 2){
re633 9:ef0907fda2f1 391 flagBeaconSyncronised = 1;
re633 9:ef0907fda2f1 392
re633 9:ef0907fda2f1 393 //Error handle. If this happens stop the piswarm
re633 9:ef0907fda2f1 394 } else {
re633 9:ef0907fda2f1 395 piswarm.set_oled_colour(255,255,255);
re633 9:ef0907fda2f1 396 piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
re633 9:ef0907fda2f1 397 piswarm.cls();
re633 9:ef0907fda2f1 398 piswarm.printf("%d %d %d",testBefore, testDuring,testAfter);
re633 9:ef0907fda2f1 399 piswarm.stop();
re633 9:ef0907fda2f1 400 wait(5);
re633 9:ef0907fda2f1 401 changeState(11);
re633 9:ef0907fda2f1 402 }
re633 9:ef0907fda2f1 403 }
re633 9:ef0907fda2f1 404 }
re633 9:ef0907fda2f1 405 }
re633 9:ef0907fda2f1 406 }
jah128 7:d03e54d9eb1c 407 }
jah128 7:d03e54d9eb1c 408
jah128 2:e806b595f9ce 409 /***************************************************************************************************************************************
jah128 2:e806b595f9ce 410 *
jah128 4:823174be9a6b 411 * Beyond this point, empty code blocks for optional functions is given
jah128 2:e806b595f9ce 412 *
jah128 4:823174be9a6b 413 * These may be left blank if not used, but should not be deleted
jah128 2:e806b595f9ce 414 *
jah128 2:e806b595f9ce 415 **************************************************************************************************************************************/
jah128 2:e806b595f9ce 416
jah128 1:37502eb3b70f 417 // Communications
jah128 1:37502eb3b70f 418
jah128 1:37502eb3b70f 419 // If using the communication stack (USE_COMMUNICATION_STACK = 1), functionality for handling user RF responses should be added to the following functions
jah128 1:37502eb3b70f 420 // If the communication stack is not being used, all radio data is sent to processRawRFData() instead
jah128 1:37502eb3b70f 421
jah128 1:37502eb3b70f 422 void handleUserRFCommand(char sender, char broadcast_message, char request_response, char id, char is_command, char function, char * data, char length){
jah128 1:37502eb3b70f 423 // A 'user' RF Command has been received: write the code here to process it
jah128 1:37502eb3b70f 424 // sender = ID of the sender, range 0 to 31
jah128 1:37502eb3b70f 425 // broadcast_message = 1 is message sent to all robots, 0 otherwise
jah128 1:37502eb3b70f 426 // request_response = 1 if a response is expected, 0 otherwise
jah128 1:37502eb3b70f 427 // id = Message ID, range 0 to 255
jah128 1:37502eb3b70f 428 // 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
jah128 1:37502eb3b70f 429 // function = The function identifier. Range 0 to 15
jah128 1:37502eb3b70f 430 // * data = Array containing extra data bytes
jah128 1:37502eb3b70f 431 // length = Length of extra data bytes held (range 0 to 57)
jah128 8:a789ef4fde52 432
jah128 7:d03e54d9eb1c 433
jah128 4:823174be9a6b 434 //Do something...
re633 10:da62735d6df9 435 if(function == 1 && gv_state == 0) {
re633 10:da62735d6df9 436 flagSystemState = 1;
re633 10:da62735d6df9 437 }
jah128 1:37502eb3b70f 438 }
jah128 1:37502eb3b70f 439
jah128 1:37502eb3b70f 440 void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){
jah128 1:37502eb3b70f 441 // A 'user' RF Response has been received: write the code here to process it
jah128 1:37502eb3b70f 442 // sender = ID of the sender, range 0 to 31
jah128 1:37502eb3b70f 443 // broadcast_message = 1 is message sent to all robots, 0 otherwise
jah128 1:37502eb3b70f 444 // success = 1 if operation successful, 0 otherwise
jah128 1:37502eb3b70f 445 // id = Message ID, range 0 to 255
jah128 1:37502eb3b70f 446 // 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
jah128 1:37502eb3b70f 447 // function = The function identifier. Range 0 to 15
jah128 1:37502eb3b70f 448 // * data = Array containing extra data bytes
jah128 1:37502eb3b70f 449 // length = Length of extra data bytes held (range 0 to 57)
jah128 4:823174be9a6b 450
jah128 4:823174be9a6b 451 //Do something...
jah128 1:37502eb3b70f 452 }
jah128 1:37502eb3b70f 453
jah128 1:37502eb3b70f 454 void processRawRFData(char * rstring, char cCount){
jah128 1:37502eb3b70f 455 // A raw RF packet has been received: write the code here to process it
jah128 1:37502eb3b70f 456 // rstring = The received packet
jah128 1:37502eb3b70f 457 // cCount = Packet length
jah128 4:823174be9a6b 458
jah128 4:823174be9a6b 459 //Do something...
jah128 0:46cd1498a39a 460 }
jah128 0:46cd1498a39a 461
jah128 1:37502eb3b70f 462 void switch_pressed() {
jah128 1:37502eb3b70f 463 //Switch(es) pressed {1 = Center 2 = Right 4 = Left 8 = Down 16 = Up}
jah128 1:37502eb3b70f 464 char switches = piswarm.get_switches();
jah128 1:37502eb3b70f 465
jah128 1:37502eb3b70f 466 //Do something...
jah128 0:46cd1498a39a 467 }