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@12:118f2b0ed8eb, 2015-08-11 (annotated)

Committer:
re633
Date:
Tue Aug 11 15:21:19 2015 +0000
Revision:
12:118f2b0ed8eb
Parent:
11:c5094a68283f
Child:
13:c18d82f62d38
Added communication with the beacon

Who changed what in which revision?

UserRevisionLine numberNew contents of line
re633 11:c5094a68283f 1 /*
re633 11:c5094a68283f 2 * Software License Agreement (BSD License)
re633 11:c5094a68283f 3 *
re633 11:c5094a68283f 4 * Copyright (c) 2015, University of York Robotics Laboratory (YRL).
re633 11:c5094a68283f 5 * All rights reserved.
re633 11:c5094a68283f 6 *
re633 11:c5094a68283f 7 * Redistribution and use in source and binary forms, with or without
re633 11:c5094a68283f 8 * modification, are permitted provided that the following conditions
re633 11:c5094a68283f 9 * are met:
re633 11:c5094a68283f 10 *
re633 11:c5094a68283f 11 * * Redistributions of source code must retain the above copyright
re633 11:c5094a68283f 12 * notice, this list of conditions and the following disclaimer.
re633 11:c5094a68283f 13 * * Redistributions in binary form must reproduce the above
re633 11:c5094a68283f 14 * copyright notice, this list of conditions and the following
re633 11:c5094a68283f 15 * disclaimer in the documentation and/or other materials provided
re633 11:c5094a68283f 16 * with the distribution.
re633 11:c5094a68283f 17 *
re633 11:c5094a68283f 18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
re633 11:c5094a68283f 19 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
re633 11:c5094a68283f 20 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
re633 11:c5094a68283f 21 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
re633 11:c5094a68283f 22 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
re633 11:c5094a68283f 23 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
re633 11:c5094a68283f 24 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
re633 11:c5094a68283f 25 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
re633 11:c5094a68283f 26 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
re633 11:c5094a68283f 27 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
re633 11:c5094a68283f 28 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
re633 11:c5094a68283f 29 * POSSIBILITY OF SUCH DAMAGE.
re633 11:c5094a68283f 30 */
re633 11:c5094a68283f 31 /**
re633 11:c5094a68283f 32 * @file main.cpp
re633 11:c5094a68283f 33 * @brief The Ticker fucntion for gaining sensor data and the main function for the PRGP Pi-Swarm Controllers.
re633 11:c5094a68283f 34 * @details In this file the main function for the Pi-Swarm Controller is defined.
re633 11:c5094a68283f 35 * @version 1.0
re633 11:c5094a68283f 36 * @author Robert
re633 11:c5094a68283f 37 * @author Evans
re633 11:c5094a68283f 38 * @date 24/07/15
re633 11:c5094a68283f 39 */
jah128 3:1aa1de26966a 40 #include "main.h" // Certain parameters can be set by changing the defines in piswarm.h
re633 9:ef0907fda2f1 41 #include "PiSwarmControllerFunctions.h"
re633 9:ef0907fda2f1 42
jah128 0:46cd1498a39a 43
jah128 1:37502eb3b70f 44 PiSwarm piswarm;
jah128 1:37502eb3b70f 45 Serial pc (USBTX,USBRX);
jah128 0:46cd1498a39a 46
re633 9:ef0907fda2f1 47 //Tickers
re633 9:ef0907fda2f1 48 Ticker ticker_25ms;
re633 9:ef0907fda2f1 49 Timer timer;
re633 10:da62735d6df9 50 Timer timerLevy;
re633 9:ef0907fda2f1 51
re633 9:ef0907fda2f1 52 //Global Variables
re633 9:ef0907fda2f1 53 uint8_t const IR_READ_PER_BEAC = 20; //The number of IR readings between beacon flashes
re633 9:ef0907fda2f1 54 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 55 int16_t volatile gv_IRValDiffs[IR_READ_PER_BEAC][8] = {0};
re633 9:ef0907fda2f1 56 uint8_t volatile beacon_detected[8] = {0};
re633 9:ef0907fda2f1 57 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 58 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 59 uint16_t const AT_BEACON_THRESHOLD = 3700;
re633 9:ef0907fda2f1 60 uint8_t volatile gv_state = 0; //This is the current state of the finite state machine
re633 11:c5094a68283f 61 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.
re633 9:ef0907fda2f1 62 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 63 int16_t g_currentHeading = 0;
re633 11:c5094a68283f 64 float BASE_SPEED = 0.4;
re633 9:ef0907fda2f1 65 int8_t g_beaconOn = 100;
re633 11:c5094a68283f 66 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
re633 11:c5094a68283f 67 int8_t tick_beacon_period_check = 100; //Is used to temporarily store the value of tick_beacon_suspected
re633 9:ef0907fda2f1 68 //Flags
re633 12:118f2b0ed8eb 69 int8_t volatile start_flag = 0;
re633 12:118f2b0ed8eb 70 int8_t volatile back_flag = 0;
re633 12:118f2b0ed8eb 71 int8_t volatile return_flag = 0;
re633 9:ef0907fda2f1 72 int8_t flagObstacle = 0;
re633 9:ef0907fda2f1 73 int8_t flagStationary = 1; //Used to determine if robot is currentl stationary or moving.
re633 9:ef0907fda2f1 74 uint8_t flagBeaconSyncronised = 0; //Set to one when the robot is synchronised with the beacon
re633 9:ef0907fda2f1 75 uint8_t volatile flagBeaconIlluminated = 0; //Should be 1 when beacon is illuminated otherwise 0
re633 10:da62735d6df9 76 uint8_t flagSetNewLevyTime = 1; //Must start as 1
re633 9:ef0907fda2f1 77 //Ticker Function*************************************************************************************
re633 9:ef0907fda2f1 78 //This function is called by a ticker every 25ms
re633 9:ef0907fda2f1 79 //The function firstly stores the background IRS values.
re633 9:ef0907fda2f1 80 //Secondly if beacon is off it uses illuminated IR sensors to estimate distances
re633 9:ef0907fda2f1 81 //Each second it will update when it believes beacon illumination time to be
re633 9:ef0907fda2f1 82 //It will work out which sensors spotted then beacon and will illuminate the Leds accordingly
re633 9:ef0907fda2f1 83 void readIRs(){
re633 9:ef0907fda2f1 84
re633 9:ef0907fda2f1 85 //Fistly update the beaconVisable flag if possible
re633 9:ef0907fda2f1 86 if(gv_counter25ms == mod8((g_beaconOn - 1), IR_READ_PER_BEAC)){
re633 9:ef0907fda2f1 87 flagBeaconIlluminated = 1;
re633 9:ef0907fda2f1 88 }
re633 9:ef0907fda2f1 89
re633 9:ef0907fda2f1 90 if(gv_counter25ms == mod8((g_beaconOn + 2), IR_READ_PER_BEAC)){
re633 9:ef0907fda2f1 91 flagBeaconIlluminated = 0;
re633 9:ef0907fda2f1 92 }
re633 9:ef0907fda2f1 93 //Firstly store background values
re633 9:ef0907fda2f1 94 //Also make a note of which point in the second did the values change most.
re633 9:ef0907fda2f1 95 //For which sensor specifically did the values change the most
re633 9:ef0907fda2f1 96 //That sensor will be used to estimate the beacon start time if a threshold value is met
re633 9:ef0907fda2f1 97 piswarm.store_background_raw_ir_values();
re633 9:ef0907fda2f1 98 int16_t IRchange = 0;
re633 9:ef0907fda2f1 99 uint8_t loopCounter = 0;
re633 9:ef0907fda2f1 100 //In this loop the raw IR values are read.
re633 9:ef0907fda2f1 101 //If the points where the IR values have increased by the greatest amount are noted as this indicates a beacon illumination
re633 9:ef0907fda2f1 102 for(loopCounter = 0; loopCounter < 8; loopCounter++) {
re633 9:ef0907fda2f1 103 gv_IRVals[gv_counter25ms][loopCounter] = piswarm.get_background_raw_ir_value(loopCounter);
re633 9:ef0907fda2f1 104 IRchange = gv_IRVals[gv_counter25ms][loopCounter]-gv_IRVals[mod8((gv_counter25ms-1),IR_READ_PER_BEAC)][loopCounter];
re633 9:ef0907fda2f1 105
re633 9:ef0907fda2f1 106 gv_IRValDiffs[gv_counter25ms][loopCounter] = IRchange;
re633 9:ef0907fda2f1 107 //printf("change %d count %d\n",IRchange);
re633 9:ef0907fda2f1 108 //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 109 if (IRchange > BEACON_SUSPECTED){
re633 11:c5094a68283f 110 tick_beacon_suspected = gv_counter25ms;
re633 9:ef0907fda2f1 111 piswarm.cls();
re633 11:c5094a68283f 112 piswarm.printf("%d",tick_beacon_suspected);
re633 9:ef0907fda2f1 113 }
re633 9:ef0907fda2f1 114 }
re633 9:ef0907fda2f1 115
re633 9:ef0907fda2f1 116 //Now store the illuminated values if the beacon is not illuminated-
re633 9:ef0907fda2f1 117 piswarm.store_illuminated_raw_ir_values();
re633 9:ef0907fda2f1 118
re633 9:ef0907fda2f1 119 //In this loop convert each raw active IR reading into a distance estimate
re633 9:ef0907fda2f1 120 for(loopCounter = 0; loopCounter < 8; loopCounter++) {
re633 9:ef0907fda2f1 121
re633 9:ef0907fda2f1 122 //Specific sensor readings converted to distances
re633 9:ef0907fda2f1 123 float temp = piswarm.get_illuminated_raw_ir_value(loopCounter);
re633 9:ef0907fda2f1 124 if(temp>3500){
re633 9:ef0907fda2f1 125 temp = 3500;
re633 9:ef0907fda2f1 126 } else if (temp < 97){
re633 9:ef0907fda2f1 127 temp = 97;
re633 9:ef0907fda2f1 128 }
re633 9:ef0907fda2f1 129 //#put this into a function
re633 9:ef0907fda2f1 130 //Switch case for robot 5
re633 9:ef0907fda2f1 131 switch(loopCounter){
re633 9:ef0907fda2f1 132 case 0:
re633 9:ef0907fda2f1 133 temp = 662 * sqrt(1/(temp-148));
re633 9:ef0907fda2f1 134 break;
re633 9:ef0907fda2f1 135 case 1:
re633 9:ef0907fda2f1 136 temp = 662 * sqrt(1/(temp-144));
re633 9:ef0907fda2f1 137 break;
re633 9:ef0907fda2f1 138 case 2:
re633 9:ef0907fda2f1 139 temp = 662 * sqrt(1/(temp-120));
re633 9:ef0907fda2f1 140 break;
re633 9:ef0907fda2f1 141 case 3:
re633 9:ef0907fda2f1 142 temp = 662 * sqrt(1/(temp-148));
re633 9:ef0907fda2f1 143 break;
re633 9:ef0907fda2f1 144 case 4:
re633 9:ef0907fda2f1 145 temp = 662 * sqrt(1/(temp-120));
re633 9:ef0907fda2f1 146 break;
re633 9:ef0907fda2f1 147 case 5:
re633 9:ef0907fda2f1 148 temp = 662 * sqrt(1/(temp-132));
re633 9:ef0907fda2f1 149 break;
re633 9:ef0907fda2f1 150 case 6:
re633 9:ef0907fda2f1 151 temp = 662 * sqrt(1/(temp-152));
re633 9:ef0907fda2f1 152 break;
re633 9:ef0907fda2f1 153 case 7:
re633 9:ef0907fda2f1 154 temp = 662 * sqrt(1/(temp-212));
re633 9:ef0907fda2f1 155 break;
re633 9:ef0907fda2f1 156 }
re633 9:ef0907fda2f1 157
re633 9:ef0907fda2f1 158 if (temp > 130){
re633 9:ef0907fda2f1 159 temp = 130;
re633 9:ef0907fda2f1 160 }
re633 9:ef0907fda2f1 161 gv_IRDistances[loopCounter] = temp;
re633 9:ef0907fda2f1 162
re633 9:ef0907fda2f1 163 }
re633 9:ef0907fda2f1 164 //reset counter after 1 second (beacon period)
re633 9:ef0907fda2f1 165 gv_counter25ms = mod8(gv_counter25ms + 1,IR_READ_PER_BEAC);
re633 9:ef0907fda2f1 166 }
re633 9:ef0907fda2f1 167
re633 9:ef0907fda2f1 168 //*******************************************************************************************************
jah128 8:a789ef4fde52 169 //This is where the program code goes.
jah128 1:37502eb3b70f 170 int main() {
re633 9:ef0907fda2f1 171 init();
re633 9:ef0907fda2f1 172 ticker_25ms.attach_us(&readIRs,25000);
re633 9:ef0907fda2f1 173 //starting point in state 11
re633 9:ef0907fda2f1 174 timer.start();
re633 10:da62735d6df9 175 timerLevy.start();
re633 9:ef0907fda2f1 176 wait(1); //Wait a second to allow IR array to be filled
re633 9:ef0907fda2f1 177
re633 10:da62735d6df9 178 //changeState(11);
jah128 8:a789ef4fde52 179
re633 9:ef0907fda2f1 180 //Controller is a finite state machine
re633 9:ef0907fda2f1 181 while(1){
re633 9:ef0907fda2f1 182
re633 9:ef0907fda2f1 183 //Waiting for signal to begin searching
re633 9:ef0907fda2f1 184 if(gv_state == 0){
re633 12:118f2b0ed8eb 185 if(start_flag == 1){
re633 12:118f2b0ed8eb 186 //Change state here after recieving a radio command
re633 10:da62735d6df9 187 changeState(11);
re633 10:da62735d6df9 188 }
re633 9:ef0907fda2f1 189 wait(1);
re633 9:ef0907fda2f1 190
re633 12:118f2b0ed8eb 191
re633 9:ef0907fda2f1 192 //Searching state
re633 9:ef0907fda2f1 193 } else if (gv_state == 11 || gv_state == 12){
re633 9:ef0907fda2f1 194
re633 12:118f2b0ed8eb 195 //Do something here on receipt of 'function 5' if necessary.
re633 12:118f2b0ed8eb 196 //As currently the home beacon will immediately switch on that is not necessary.
re633 12:118f2b0ed8eb 197
re633 12:118f2b0ed8eb 198 //Determine if suspected beacon is actually the beacon.
re633 9:ef0907fda2f1 199 //This is done by checking the period between flashes matches the beacon period
re633 11:c5094a68283f 200 if(tick_beacon_suspected != 100){
re633 9:ef0907fda2f1 201 //piswarm.stop();
re633 9:ef0907fda2f1 202 //When the beacon flag is first raised store its value and reset it
re633 11:c5094a68283f 203 if(tick_beacon_period_check == 100){
re633 11:c5094a68283f 204 tick_beacon_period_check = tick_beacon_suspected;
re633 11:c5094a68283f 205 tick_beacon_suspected = 100;
re633 9:ef0907fda2f1 206 //Check the timing of the latest jump with the last one to see if period matches the Beacon.
re633 9:ef0907fda2f1 207 } else {
re633 9:ef0907fda2f1 208 piswarm.locate(0,1);
re633 11:c5094a68283f 209 piswarm.printf("%d %d",tick_beacon_period_check,tick_beacon_suspected);
re633 11:c5094a68283f 210 //printf("%d %d *********************************",tick_beacon_period_check,tick_beacon_suspected);
re633 9:ef0907fda2f1 211 //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 11:c5094a68283f 212 int8_t test = (tick_beacon_period_check - tick_beacon_suspected);
re633 9:ef0907fda2f1 213
re633 9:ef0907fda2f1 214 test = test * test;
re633 9:ef0907fda2f1 215
re633 9:ef0907fda2f1 216 //if test is low then identify the beacon as the cause of the flags
re633 9:ef0907fda2f1 217 if(test < 2){
re633 9:ef0907fda2f1 218 //Beacon found change to state 2
re633 11:c5094a68283f 219 g_beaconOn = tick_beacon_period_check; //update the global variable that stores when beacon flashes occur
re633 9:ef0907fda2f1 220
re633 9:ef0907fda2f1 221 wait(2);
re633 9:ef0907fda2f1 222 changeState(2);
re633 9:ef0907fda2f1 223 } else {
re633 9:ef0907fda2f1 224 //Reset the flag to try again
re633 11:c5094a68283f 225 tick_beacon_period_check = 100;
re633 9:ef0907fda2f1 226 }
re633 9:ef0907fda2f1 227 }
re633 9:ef0907fda2f1 228 }
re633 9:ef0907fda2f1 229
re633 9:ef0907fda2f1 230 if(gv_state == 11){
re633 9:ef0907fda2f1 231 //Secondly if obstacles are detected ahead then execute a random turn.
re633 9:ef0907fda2f1 232 if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
re633 9:ef0907fda2f1 233 piswarm.stop();
re633 10:da62735d6df9 234 piswarm.cls();
re633 10:da62735d6df9 235 piswarm.printf("ob R");
re633 10:da62735d6df9 236 piswarm.play_tune("CC",1);
re633 9:ef0907fda2f1 237 wait(0.1);
re633 9:ef0907fda2f1 238 flagObstacle = 1;
re633 9:ef0907fda2f1 239 changeState(12);
re633 9:ef0907fda2f1 240 } else if(gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
re633 9:ef0907fda2f1 241 piswarm.stop();
re633 10:da62735d6df9 242 piswarm.cls();
re633 10:da62735d6df9 243 piswarm.printf("ob L");
re633 10:da62735d6df9 244 piswarm.play_tune("CC",1);
re633 9:ef0907fda2f1 245 wait(0.1);
re633 9:ef0907fda2f1 246 flagObstacle = 2;
re633 9:ef0907fda2f1 247 changeState(12);
re633 10:da62735d6df9 248
re633 10:da62735d6df9 249 //Otherwise continue moving forward until distance determined by levy algorithm is calculated.
re633 11:c5094a68283f 250 } else if(timerLevy.read_us() > levy_target_time_us){
re633 10:da62735d6df9 251 flagSetNewLevyTime = 1;
re633 10:da62735d6df9 252 piswarm.play_tune("G",1);
re633 10:da62735d6df9 253 changeState(12);
re633 9:ef0907fda2f1 254
re633 9:ef0907fda2f1 255 } else if (flagStationary == 1){
re633 9:ef0907fda2f1 256
re633 11:c5094a68283f 257 piswarm.forward(BASE_SPEED);
re633 9:ef0907fda2f1 258 flagStationary = 0;
re633 9:ef0907fda2f1 259 }
re633 9:ef0907fda2f1 260
re633 9:ef0907fda2f1 261 } else if(gv_state == 12){
re633 9:ef0907fda2f1 262 piswarm.stop();//Stop the robot.
re633 9:ef0907fda2f1 263 flagStationary = 1; //update this flag
re633 9:ef0907fda2f1 264 int16_t randomAngle;
re633 9:ef0907fda2f1 265 //If sent here beacuse of obstacle find angle between -180 to -90 and 90 to 180
re633 9:ef0907fda2f1 266 if(flagObstacle == 1){
re633 10:da62735d6df9 267 randomAngle = rand()%90 - 135;
re633 9:ef0907fda2f1 268
re633 9:ef0907fda2f1 269 } else if(flagObstacle == 2){
re633 10:da62735d6df9 270 randomAngle = rand()%90 + 45;
re633 10:da62735d6df9 271
re633 10:da62735d6df9 272 //Otherwise if here due to levy walk: turn to any random angle
re633 9:ef0907fda2f1 273 } else {
re633 10:da62735d6df9 274 randomAngle = rand()%360 - 180;
re633 9:ef0907fda2f1 275 }
re633 9:ef0907fda2f1 276 turnDegrees(randomAngle); //Make the turn
re633 10:da62735d6df9 277 wait(0.1);
re633 10:da62735d6df9 278 /* if(gv_IRDistances[0] < 70 || gv_IRDistances[1] < 70 || gv_IRDistances[6] < 70 || gv_IRDistances[7] < 70){
re633 10:da62735d6df9 279 //do nothing. Aim is that robot will keep turning the same way until clear of obstacle
re633 10:da62735d6df9 280 //could put a count in here to doemergency escape manouvre if necessary
re633 10:da62735d6df9 281 piswarm.play_tune("CC",1);
re633 10:da62735d6df9 282 } else { */
re633 10:da62735d6df9 283 flagObstacle = 0;
re633 10:da62735d6df9 284 changeState(11);//Move back into state 11
re633 10:da62735d6df9 285
re633 9:ef0907fda2f1 286 }
re633 9:ef0907fda2f1 287
re633 9:ef0907fda2f1 288
re633 9:ef0907fda2f1 289 //Beacon found state
re633 9:ef0907fda2f1 290 } else if (gv_state == 2){
re633 12:118f2b0ed8eb 291
re633 12:118f2b0ed8eb 292 //Do something here on receipt of 'function 5' if necessary.
re633 12:118f2b0ed8eb 293 //As currently the home beacon will immediately switch on that is not necessary.
re633 12:118f2b0ed8eb 294
re633 9:ef0907fda2f1 295 int16_t maxValue[2] = {0,100}; //Value and sensor position
re633 9:ef0907fda2f1 296 //wait(1);
re633 9:ef0907fda2f1 297 uint8_t loopCounter = 0;
re633 9:ef0907fda2f1 298
re633 9:ef0907fda2f1 299 //If beacon visible
re633 9:ef0907fda2f1 300 if(flagBeaconSyncronised == 1){
re633 9:ef0907fda2f1 301
re633 9:ef0907fda2f1 302 //Firstly check beacon is still visible
re633 9:ef0907fda2f1 303 flagBeaconSyncronised = 0;
re633 9:ef0907fda2f1 304 //Update array concerning which IRs can see the beacon
re633 9:ef0907fda2f1 305 for(loopCounter = 0; loopCounter<8; loopCounter++) {
re633 9:ef0907fda2f1 306
re633 9:ef0907fda2f1 307 //Find which sensor has the highest reading
re633 9:ef0907fda2f1 308 if( gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED) {
re633 9:ef0907fda2f1 309 if(gv_IRVals[g_beaconOn][loopCounter] > maxValue[0]){
re633 9:ef0907fda2f1 310 maxValue[0] = gv_IRVals[g_beaconOn][loopCounter];
re633 9:ef0907fda2f1 311 maxValue[1] = loopCounter;
re633 9:ef0907fda2f1 312 }
re633 9:ef0907fda2f1 313 flagBeaconSyncronised = 1; //This will remain as one so long as at least on sensor can see beacon
re633 9:ef0907fda2f1 314 }
re633 9:ef0907fda2f1 315 }
re633 9:ef0907fda2f1 316
re633 9:ef0907fda2f1 317 //Only do this if beacon still visible
re633 9:ef0907fda2f1 318 if(flagBeaconSyncronised == 1){
re633 9:ef0907fda2f1 319
re633 9:ef0907fda2f1 320 //If the adjacent two sensors are above the threshold too then they can also be marked as illuminated
re633 9:ef0907fda2f1 321 for(loopCounter = 0; loopCounter<8; loopCounter++){
re633 9:ef0907fda2f1 322
re633 9:ef0907fda2f1 323 //reset all beacon detected values
re633 9:ef0907fda2f1 324 beacon_detected[loopCounter] = 0;
re633 9:ef0907fda2f1 325
re633 9:ef0907fda2f1 326 if(abs(maxValue[1] - loopCounter)< 3 || abs(maxValue[1] + 8 - loopCounter)< 3 || abs(maxValue[1] - 8 - loopCounter)< 3) {
re633 9:ef0907fda2f1 327 if(gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED){
re633 9:ef0907fda2f1 328 beacon_detected[loopCounter] = 1;
re633 9:ef0907fda2f1 329 }
re633 9:ef0907fda2f1 330 }
re633 9:ef0907fda2f1 331 }
re633 9:ef0907fda2f1 332
re633 9:ef0907fda2f1 333
re633 9:ef0907fda2f1 334 //Update the piswarm LEDS so the ones that can see the beacon are on.
re633 9:ef0907fda2f1 335 piswarm.set_oleds(beacon_detected[0]||beacon_detected[1],
re633 9:ef0907fda2f1 336 beacon_detected[1]||beacon_detected[2],
re633 9:ef0907fda2f1 337 beacon_detected[2],
re633 9:ef0907fda2f1 338 beacon_detected[3],
re633 9:ef0907fda2f1 339 0,
re633 9:ef0907fda2f1 340 beacon_detected[4],
re633 9:ef0907fda2f1 341 beacon_detected[5],
re633 9:ef0907fda2f1 342 beacon_detected[5]||beacon_detected[6],
re633 9:ef0907fda2f1 343 beacon_detected[6]||beacon_detected[7],
re633 9:ef0907fda2f1 344 beacon_detected[7]||beacon_detected[0]);
re633 9:ef0907fda2f1 345
re633 9:ef0907fda2f1 346 //If the max IR value is below a threshold then move toward beacon. Else change state
re633 9:ef0907fda2f1 347 if(maxValue[0] < AT_BEACON_THRESHOLD){
re633 9:ef0907fda2f1 348
re633 9:ef0907fda2f1 349 //Calculate the heading of Pi-Swarm Relative to beacon
re633 9:ef0907fda2f1 350 //printf("%d ",g_currentHeading);
re633 9:ef0907fda2f1 351 calculateNewHeading();
re633 9:ef0907fda2f1 352 printf("%d ",g_currentHeading);
re633 9:ef0907fda2f1 353 if(g_currentHeading > 5 ||g_currentHeading < -5){
re633 9:ef0907fda2f1 354 turnDegrees(-g_currentHeading);
re633 9:ef0907fda2f1 355 }
re633 9:ef0907fda2f1 356 printf("%d\n",g_currentHeading);
re633 9:ef0907fda2f1 357 //}
re633 9:ef0907fda2f1 358
re633 9:ef0907fda2f1 359
re633 9:ef0907fda2f1 360
re633 9:ef0907fda2f1 361 //If the beacon is not currently on but obstacle detected then do obstacle avoidance
re633 9:ef0907fda2f1 362
re633 9:ef0907fda2f1 363 if(flagBeaconIlluminated == 0){
re633 9:ef0907fda2f1 364 if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
re633 11:c5094a68283f 365 turnDegrees(-90);
re633 9:ef0907fda2f1 366 } else if (gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
re633 11:c5094a68283f 367 turnDegrees(90);
re633 9:ef0907fda2f1 368 }
re633 9:ef0907fda2f1 369 }
re633 9:ef0907fda2f1 370
re633 9:ef0907fda2f1 371
re633 9:ef0907fda2f1 372 piswarm.forward(0.2);
re633 9:ef0907fda2f1 373 wait(1);
re633 9:ef0907fda2f1 374 //Should be at beacon
re633 9:ef0907fda2f1 375 } else {
re633 9:ef0907fda2f1 376 piswarm.stop();
re633 9:ef0907fda2f1 377
re633 12:118f2b0ed8eb 378 //If either of these flags is one then the beacon should be the home beacon so change to state 4.
re633 12:118f2b0ed8eb 379 if(return_flag == 1 || back_flag == 1){
re633 12:118f2b0ed8eb 380 changeState(4);
re633 12:118f2b0ed8eb 381 } else {
re633 12:118f2b0ed8eb 382 changeState(3);
re633 12:118f2b0ed8eb 383 }
re633 9:ef0907fda2f1 384 }
re633 9:ef0907fda2f1 385 }
re633 9:ef0907fda2f1 386 //Else need to syncronise with beacon
re633 9:ef0907fda2f1 387 } else {
re633 9:ef0907fda2f1 388
re633 9:ef0907fda2f1 389 while(flagBeaconSyncronised == 0){
re633 9:ef0907fda2f1 390 //Sychronise the ticker with the beacon
re633 9:ef0907fda2f1 391
re633 9:ef0907fda2f1 392 uint8_t testBefore = 0;
re633 9:ef0907fda2f1 393 uint8_t testDuring = 0;
re633 9:ef0907fda2f1 394 uint8_t testAfter = 0;
re633 9:ef0907fda2f1 395 for(loopCounter = 0; loopCounter < 8; loopCounter++){
re633 9:ef0907fda2f1 396 if (gv_IRValDiffs[mod8((g_beaconOn - 1),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){
re633 9:ef0907fda2f1 397 testBefore = 1;
re633 9:ef0907fda2f1 398 }
re633 9:ef0907fda2f1 399 if (gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED){
re633 9:ef0907fda2f1 400 testDuring = 1;
re633 9:ef0907fda2f1 401 }
re633 9:ef0907fda2f1 402 if (gv_IRValDiffs[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){
re633 9:ef0907fda2f1 403 testAfter = 1;
re633 9:ef0907fda2f1 404 }
re633 9:ef0907fda2f1 405 if (gv_IRValDiffs[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] < -BEACON_SUSPECTED){
re633 9:ef0907fda2f1 406 testAfter = 2;
re633 9:ef0907fda2f1 407 }
re633 9:ef0907fda2f1 408 }
re633 9:ef0907fda2f1 409 //Firstly if the beacon is not detected by any of the sensors then change state back to search
re633 9:ef0907fda2f1 410 if(testBefore == 0 && testDuring == 0 && testAfter == 0){
re633 9:ef0907fda2f1 411 changeState(11);
re633 9:ef0907fda2f1 412 flagBeaconSyncronised = 1;//to exit while loop
re633 9:ef0907fda2f1 413
re633 9:ef0907fda2f1 414 //If the tick before g_beaconOn is detecting the change caused by the flash change the value of g_beaconOn
re633 9:ef0907fda2f1 415 } else if(testBefore == 1){
re633 9:ef0907fda2f1 416 g_beaconOn = g_beaconOn - 1;
re633 9:ef0907fda2f1 417
re633 9:ef0907fda2f1 418 //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 419 } else if(testBefore == 0 && testDuring == 1 && testAfter == 1){
re633 9:ef0907fda2f1 420 ticker_25ms.detach();
re633 9:ef0907fda2f1 421 timer.reset();
re633 9:ef0907fda2f1 422 while(timer.read_ms() < 100){};
re633 9:ef0907fda2f1 423 ticker_25ms.attach_us(&readIRs,25000);
re633 9:ef0907fda2f1 424 wait(1);
re633 9:ef0907fda2f1 425
re633 9:ef0907fda2f1 426 //If successful the set flag
re633 9:ef0907fda2f1 427 } else if (testBefore == 0 && testDuring == 1 && testAfter == 2){
re633 9:ef0907fda2f1 428 flagBeaconSyncronised = 1;
re633 9:ef0907fda2f1 429
re633 9:ef0907fda2f1 430 //Error handle. If this happens stop the piswarm
re633 9:ef0907fda2f1 431 } else {
re633 9:ef0907fda2f1 432 piswarm.set_oled_colour(255,255,255);
re633 9:ef0907fda2f1 433 piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
re633 9:ef0907fda2f1 434 piswarm.cls();
re633 9:ef0907fda2f1 435 piswarm.printf("%d %d %d",testBefore, testDuring,testAfter);
re633 9:ef0907fda2f1 436 piswarm.stop();
re633 12:118f2b0ed8eb 437 wait(2);
re633 9:ef0907fda2f1 438 changeState(11);
re633 9:ef0907fda2f1 439 }
re633 9:ef0907fda2f1 440 }
re633 12:118f2b0ed8eb 441 }
re633 12:118f2b0ed8eb 442 //At target Beacon.
re633 12:118f2b0ed8eb 443 //Broadcast target beacon found and wait.
re633 12:118f2b0ed8eb 444 } else if (gv_state == 3){
re633 12:118f2b0ed8eb 445
re633 12:118f2b0ed8eb 446
re633 12:118f2b0ed8eb 447 piswarm.stop();
re633 12:118f2b0ed8eb 448 piswarm.set_oled_colour(150,255,0);
re633 12:118f2b0ed8eb 449 piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
re633 12:118f2b0ed8eb 450
re633 12:118f2b0ed8eb 451 //The value of function will change depending on the type of TAG used
re633 12:118f2b0ed8eb 452 char function = 2;
re633 12:118f2b0ed8eb 453 char message[2] = {"0"};
re633 12:118f2b0ed8eb 454 broadcast_user_rf_command(function,message,1);
re633 12:118f2b0ed8eb 455 wait(0.5);
re633 12:118f2b0ed8eb 456 broadcast_user_rf_command(function,message,1);
re633 12:118f2b0ed8eb 457 wait(0.5);
re633 12:118f2b0ed8eb 458 broadcast_user_rf_command(function,message,1);
re633 12:118f2b0ed8eb 459
re633 12:118f2b0ed8eb 460 while(back_flag == 0){
re633 12:118f2b0ed8eb 461 //Wait for the back flag to change
re633 12:118f2b0ed8eb 462 wait(0.5);
re633 12:118f2b0ed8eb 463 }
re633 12:118f2b0ed8eb 464
re633 12:118f2b0ed8eb 465 //Return to beacon search state but now robot is lookling for the home beacon.
re633 12:118f2b0ed8eb 466 changeState(11);
re633 12:118f2b0ed8eb 467
re633 12:118f2b0ed8eb 468 //Back at home area. Stop and wait.
re633 12:118f2b0ed8eb 469 } else if (gv_state == 4){
re633 12:118f2b0ed8eb 470 piswarm.stop();
re633 12:118f2b0ed8eb 471 piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
re633 12:118f2b0ed8eb 472 while(1){
re633 12:118f2b0ed8eb 473 piswarm.set_oled_colour(0,150,0);
re633 12:118f2b0ed8eb 474 wait(0.2);
re633 12:118f2b0ed8eb 475 piswarm.set_oled_colour(150,0,0);
re633 12:118f2b0ed8eb 476 wait(0.2);
re633 12:118f2b0ed8eb 477 piswarm.set_oled_colour(0,0,150);
re633 12:118f2b0ed8eb 478 wait(0.2);
re633 12:118f2b0ed8eb 479 }
re633 9:ef0907fda2f1 480 }
re633 9:ef0907fda2f1 481 }
jah128 7:d03e54d9eb1c 482 }
jah128 7:d03e54d9eb1c 483
jah128 2:e806b595f9ce 484 /***************************************************************************************************************************************
jah128 2:e806b595f9ce 485 *
jah128 4:823174be9a6b 486 * Beyond this point, empty code blocks for optional functions is given
jah128 2:e806b595f9ce 487 *
jah128 4:823174be9a6b 488 * These may be left blank if not used, but should not be deleted
jah128 2:e806b595f9ce 489 *
jah128 2:e806b595f9ce 490 **************************************************************************************************************************************/
jah128 2:e806b595f9ce 491
jah128 1:37502eb3b70f 492 // Communications
jah128 1:37502eb3b70f 493
jah128 1:37502eb3b70f 494 // 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 495 // If the communication stack is not being used, all radio data is sent to processRawRFData() instead
jah128 1:37502eb3b70f 496
jah128 1:37502eb3b70f 497 void handleUserRFCommand(char sender, char broadcast_message, char request_response, char id, char is_command, char function, char * data, char length){
jah128 1:37502eb3b70f 498 // A 'user' RF Command has been received: write the code here to process it
jah128 1:37502eb3b70f 499 // sender = ID of the sender, range 0 to 31
jah128 1:37502eb3b70f 500 // broadcast_message = 1 is message sent to all robots, 0 otherwise
jah128 1:37502eb3b70f 501 // request_response = 1 if a response is expected, 0 otherwise
jah128 1:37502eb3b70f 502 // id = Message ID, range 0 to 255
jah128 1:37502eb3b70f 503 // 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 504 // function = The function identifier. Range 0 to 15
jah128 1:37502eb3b70f 505 // * data = Array containing extra data bytes
jah128 1:37502eb3b70f 506 // length = Length of extra data bytes held (range 0 to 57)
jah128 8:a789ef4fde52 507
jah128 7:d03e54d9eb1c 508
jah128 4:823174be9a6b 509 //Do something...
re633 12:118f2b0ed8eb 510 piswarm.cls();
re633 12:118f2b0ed8eb 511 piswarm.locate(0,0);
re633 12:118f2b0ed8eb 512 piswarm.printf("URF:%d",function);
re633 12:118f2b0ed8eb 513 if(length > 0) {
re633 12:118f2b0ed8eb 514 piswarm.locate(0,1);
re633 12:118f2b0ed8eb 515 piswarm.printf("%s",data);
re633 12:118f2b0ed8eb 516 }
re633 12:118f2b0ed8eb 517
re633 12:118f2b0ed8eb 518 if(function == 1){
re633 12:118f2b0ed8eb 519 start_flag = 1;
re633 12:118f2b0ed8eb 520 }
re633 12:118f2b0ed8eb 521
re633 12:118f2b0ed8eb 522 if(function == 5){
re633 12:118f2b0ed8eb 523 return_flag = 1;
re633 12:118f2b0ed8eb 524 }
re633 12:118f2b0ed8eb 525 if(function == 6){
re633 12:118f2b0ed8eb 526 back_flag = 1;
re633 10:da62735d6df9 527 }
jah128 1:37502eb3b70f 528 }
jah128 1:37502eb3b70f 529
re633 12:118f2b0ed8eb 530 // This function is used to send the RF message:
re633 12:118f2b0ed8eb 531 void broadcast_user_rf_command(int function, char * message, int length)
re633 12:118f2b0ed8eb 532 {
re633 12:118f2b0ed8eb 533 //This function augments the communications stack
re633 12:118f2b0ed8eb 534 //It sends a 'user' RF command to all members (ie target_id = 0)
re633 12:118f2b0ed8eb 535 //It sends a 'request', not a 'command', meaning it will still be handled if commands are disabled (RF_ALLOW_COMMANDS set to 0, recommended)
re633 12:118f2b0ed8eb 536 //It takes three inputs:
re633 12:118f2b0ed8eb 537 // * function (an integer from 0 to 15)
re633 12:118f2b0ed8eb 538 // * message (a char array)
re633 12:118f2b0ed8eb 539 // * length (length of message in bytes)
re633 12:118f2b0ed8eb 540 send_rf_message(0,48+(function % 16),message,length);
re633 12:118f2b0ed8eb 541 }
re633 12:118f2b0ed8eb 542
jah128 1:37502eb3b70f 543 void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){
jah128 1:37502eb3b70f 544 // A 'user' RF Response has been received: write the code here to process it
jah128 1:37502eb3b70f 545 // sender = ID of the sender, range 0 to 31
jah128 1:37502eb3b70f 546 // broadcast_message = 1 is message sent to all robots, 0 otherwise
jah128 1:37502eb3b70f 547 // success = 1 if operation successful, 0 otherwise
jah128 1:37502eb3b70f 548 // id = Message ID, range 0 to 255
jah128 1:37502eb3b70f 549 // 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 550 // function = The function identifier. Range 0 to 15
jah128 1:37502eb3b70f 551 // * data = Array containing extra data bytes
jah128 1:37502eb3b70f 552 // length = Length of extra data bytes held (range 0 to 57)
jah128 4:823174be9a6b 553
jah128 4:823174be9a6b 554 //Do something...
jah128 1:37502eb3b70f 555 }
jah128 1:37502eb3b70f 556
jah128 1:37502eb3b70f 557 void processRawRFData(char * rstring, char cCount){
jah128 1:37502eb3b70f 558 // A raw RF packet has been received: write the code here to process it
jah128 1:37502eb3b70f 559 // rstring = The received packet
jah128 1:37502eb3b70f 560 // cCount = Packet length
jah128 4:823174be9a6b 561
jah128 4:823174be9a6b 562 //Do something...
jah128 0:46cd1498a39a 563 }
jah128 0:46cd1498a39a 564
jah128 1:37502eb3b70f 565 void switch_pressed() {
jah128 1:37502eb3b70f 566 //Switch(es) pressed {1 = Center 2 = Right 4 = Left 8 = Down 16 = Up}
jah128 1:37502eb3b70f 567 char switches = piswarm.get_switches();
jah128 1:37502eb3b70f 568
jah128 1:37502eb3b70f 569 //Do something...
jah128 0:46cd1498a39a 570 }