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
main.cpp@14:fc406dfff94f, 2015-08-23 (annotated)
- Committer:
- homero
- Date:
- Sun Aug 23 16:27:14 2015 +0000
- Revision:
- 14:fc406dfff94f
- Parent:
- 13:c18d82f62d38
- Child:
- 15:f7a8989a3cd3
indentation error
Who changed what in which revision?
| User | Revision | Line number | New 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 |
| homero | 13:c18d82f62d38 | 36 | * @author Robert Evans |
| re633 | 11:c5094a68283f | 37 | * @date 24/07/15 |
| homero | 13:c18d82f62d38 | 38 | * @version 2.0 |
| homero | 13:c18d82f62d38 | 39 | * @author Homero Silva |
| homero | 13:c18d82f62d38 | 40 | * @date 16/08/15 |
| homero | 13:c18d82f62d38 | 41 | * @ultrasonic sensor added |
| homero | 13:c18d82f62d38 | 42 | * @version 3.0 |
| homero | 13:c18d82f62d38 | 43 | * @author Robert Evans |
| homero | 13:c18d82f62d38 | 44 | * @date 17/08/15 |
| homero | 13:c18d82f62d38 | 45 | * @communication added |
| re633 | 11:c5094a68283f | 46 | */ |
| jah128 | 3:1aa1de26966a | 47 | #include "main.h" // Certain parameters can be set by changing the defines in piswarm.h |
| homero | 14:fc406dfff94f | 48 | #include "pi_swarm_functions.h" |
| homero | 13:c18d82f62d38 | 49 | #include "hcsr04.h" |
| jah128 | 0:46cd1498a39a | 50 | |
| jah128 | 1:37502eb3b70f | 51 | PiSwarm piswarm; |
| jah128 | 1:37502eb3b70f | 52 | Serial pc (USBTX,USBRX); |
| homero | 13:c18d82f62d38 | 53 | HCSR04 usensor(p30,p14); |
| jah128 | 0:46cd1498a39a | 54 | |
| re633 | 9:ef0907fda2f1 | 55 | //Tickers |
| homero | 14:fc406dfff94f | 56 | Ticker ticker_25ms; //Ticker used to periodically obtain new IR readings. |
| homero | 14:fc406dfff94f | 57 | Ticker ticker_ultrasonic50ms; //Ticker used to periodically obtain a new ultrasonic reading. |
| homero | 13:c18d82f62d38 | 58 | |
| homero | 13:c18d82f62d38 | 59 | //Timeouts |
| homero | 14:fc406dfff94f | 60 | Timeout tickChangeTimeout; //Timeout used to adjust IR read ticker phase to enable to beacon syncronisation. |
| homero | 14:fc406dfff94f | 61 | Timeout broadcastTimeout; //Timeout used for broadcasting messages multiple times with a set interval. |
| homero | 13:c18d82f62d38 | 62 | |
| homero | 13:c18d82f62d38 | 63 | //Timers |
| homero | 14:fc406dfff94f | 64 | Timer turn_timer; //Timer used to turn a user specified number of degrees. |
| homero | 14:fc406dfff94f | 65 | Timer levy_timer; //Timer used to control time between levy walk steps. |
| re633 | 9:ef0907fda2f1 | 66 | |
| re633 | 9:ef0907fda2f1 | 67 | //Global Variables |
| homero | 14:fc406dfff94f | 68 | |
| homero | 14:fc406dfff94f | 69 | //Individucal Pi-Swarm variables: speed and motor trim etc |
| homero | 14:fc406dfff94f | 70 | float BASE_SPEED = 0.6; //The input to the motor functions when the robot is moving forward and does not detect obstacles. |
| homero | 14:fc406dfff94f | 71 | float r_mot_scaler = 1; //This variable is allows particular pi swarms to be adjusted to travel in a straight line. |
| homero | 14:fc406dfff94f | 72 | float l_mot_scaler = 1; //This variable is allows particular pi swarms to be adjusted to travel in a straight line. |
| homero | 14:fc406dfff94f | 73 | uint16_t at_beacon_threshold = 3800; //A raw IR reading greater than this value indicates the robot is at a beacon. |
| homero | 14:fc406dfff94f | 74 | int16_t g_currentHeading = 0; //The current heading of the Pi-swarm relative to the IR beacon. |
| homero | 14:fc406dfff94f | 75 | uint8_t robot_id; //The id unique to each Pi-Swarm robot |
| homero | 14:fc406dfff94f | 76 | |
| homero | 14:fc406dfff94f | 77 | //Finite state machine information |
| homero | 14:fc406dfff94f | 78 | uint8_t volatile gv_state = States::READY_TO_START; //This is the current state of the finite state machine |
| homero | 14:fc406dfff94f | 79 | 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. |
| homero | 14:fc406dfff94f | 80 | |
| homero | 14:fc406dfff94f | 81 | //IR Readings + beacon syncronisation variables |
| re633 | 9:ef0907fda2f1 | 82 | uint8_t const IR_READ_PER_BEAC = 20; //The number of IR readings between beacon flashes |
| re633 | 9:ef0907fda2f1 | 83 | 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 |
| homero | 14:fc406dfff94f | 84 | int16_t volatile gv_IRValDiffs[IR_READ_PER_BEAC][8] = {0};//The difference between consective IR readings. |
| homero | 14:fc406dfff94f | 85 | int16_t volatile gv_IRValDiffsTwo[IR_READ_PER_BEAC][8] = {0};//The difference between IR readings seperated by two ticks |
| homero | 14:fc406dfff94f | 86 | uint8_t volatile beacon_detected[8] = {0}; //An IR sensor is judged to have a detected the beacon in a valid way the corresponding array variable will be set to 1, else 0. |
| re633 | 9:ef0907fda2f1 | 87 | 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) |
| homero | 14:fc406dfff94f | 88 | int8_t g_beaconOn = 100; //The first tick at which the beacon is illuminated |
| homero | 13:c18d82f62d38 | 89 | 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. |
| homero | 14:fc406dfff94f | 90 | 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 | 11:c5094a68283f | 91 | 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 | 92 | int8_t tick_beacon_period_check = 100; //Is used to temporarily store the value of tick_beacon_suspected |
| homero | 14:fc406dfff94f | 93 | uint8_t beacon_syncronised_flag = 0; //Set to one when the robot is synchronised with the beacon |
| homero | 14:fc406dfff94f | 94 | uint8_t volatile beacon_illuminated_flag = 0; //Should be 1 when beacon is illuminated otherwise 0 |
| homero | 14:fc406dfff94f | 95 | |
| homero | 14:fc406dfff94f | 96 | //Ultrasonic Readings |
| homero | 14:fc406dfff94f | 97 | float volatile distance_ultrasonic_sensor = 1000; //Here is stored the latest reading from the ultrasonic sensor in mm from 1 -800. 1000 is an error value. |
| homero | 14:fc406dfff94f | 98 | |
| homero | 14:fc406dfff94f | 99 | //Levy walk variables |
| homero | 14:fc406dfff94f | 100 | 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. |
| homero | 14:fc406dfff94f | 101 | uint8_t g_back_count = 0; //After a set number of Levy walk turns the robot will move backward. This variable monitors how many Levy turns since the last backward move. |
| homero | 14:fc406dfff94f | 102 | uint8_t set_new_levy_time_flag = 1; //Must start as 1. If fwd move state is entered following a levy turn the flag will be 1 indicating a new time should be set. |
| homero | 14:fc406dfff94f | 103 | |
| homero | 14:fc406dfff94f | 104 | |
| homero | 14:fc406dfff94f | 105 | //Communication |
| homero | 14:fc406dfff94f | 106 | uint8_t broadcasted_flag = 1; //Set to one when a message has been broadcast. 0 when about to broadcast a new message |
| homero | 14:fc406dfff94f | 107 | uint8_t broadcasted_count = 0; //The number of messages that have been broadcasted in one batch |
| homero | 14:fc406dfff94f | 108 | int8_t volatile start_flag = 1; //Set to 1 on receiving a command by RF. Indicates robot can start searching for target beacon. |
| homero | 14:fc406dfff94f | 109 | 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. |
| homero | 14:fc406dfff94f | 110 | 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. |
| homero | 13:c18d82f62d38 | 111 | |
| re633 | 9:ef0907fda2f1 | 112 | //Ticker Function************************************************************************************* |
| re633 | 9:ef0907fda2f1 | 113 | //This function is called by a ticker every 25ms |
| re633 | 9:ef0907fda2f1 | 114 | //The function firstly stores the background IRS values. |
| re633 | 9:ef0907fda2f1 | 115 | //Secondly if beacon is off it uses illuminated IR sensors to estimate distances |
| re633 | 9:ef0907fda2f1 | 116 | //Each second it will update when it believes beacon illumination time to be |
| re633 | 9:ef0907fda2f1 | 117 | //It will work out which sensors spotted then beacon and will illuminate the Leds accordingly |
| re633 | 9:ef0907fda2f1 | 118 | void readIRs(){ |
| re633 | 9:ef0907fda2f1 | 119 | |
| re633 | 9:ef0907fda2f1 | 120 | //Fistly update the beaconVisable flag if possible |
| re633 | 9:ef0907fda2f1 | 121 | if(gv_counter25ms == mod8((g_beaconOn - 1), IR_READ_PER_BEAC)){ |
| homero | 14:fc406dfff94f | 122 | beacon_illuminated_flag = 1; |
| re633 | 9:ef0907fda2f1 | 123 | } |
| re633 | 9:ef0907fda2f1 | 124 | |
| re633 | 9:ef0907fda2f1 | 125 | if(gv_counter25ms == mod8((g_beaconOn + 2), IR_READ_PER_BEAC)){ |
| homero | 14:fc406dfff94f | 126 | beacon_illuminated_flag = 0; |
| re633 | 9:ef0907fda2f1 | 127 | } |
| re633 | 9:ef0907fda2f1 | 128 | //Firstly store background values |
| re633 | 9:ef0907fda2f1 | 129 | //Also make a note of which point in the second did the values change most. |
| re633 | 9:ef0907fda2f1 | 130 | //For which sensor specifically did the values change the most |
| re633 | 9:ef0907fda2f1 | 131 | //That sensor will be used to estimate the beacon start time if a threshold value is met |
| re633 | 9:ef0907fda2f1 | 132 | piswarm.store_background_raw_ir_values(); |
| re633 | 9:ef0907fda2f1 | 133 | int16_t IRchange = 0; |
| homero | 13:c18d82f62d38 | 134 | int16_t IRchange2 = 0; |
| re633 | 9:ef0907fda2f1 | 135 | uint8_t loopCounter = 0; |
| re633 | 9:ef0907fda2f1 | 136 | //In this loop the raw IR values are read. |
| re633 | 9:ef0907fda2f1 | 137 | //If the points where the IR values have increased by the greatest amount are noted as this indicates a beacon illumination |
| re633 | 9:ef0907fda2f1 | 138 | for(loopCounter = 0; loopCounter < 8; loopCounter++) { |
| re633 | 9:ef0907fda2f1 | 139 | gv_IRVals[gv_counter25ms][loopCounter] = piswarm.get_background_raw_ir_value(loopCounter); |
| re633 | 9:ef0907fda2f1 | 140 | |
| homero | 13:c18d82f62d38 | 141 | IRchange = gv_IRVals[gv_counter25ms][loopCounter]-gv_IRVals[mod8((gv_counter25ms-1),IR_READ_PER_BEAC)][loopCounter]; |
| homero | 13:c18d82f62d38 | 142 | IRchange2 = gv_IRVals[gv_counter25ms][loopCounter]-gv_IRVals[mod8((gv_counter25ms-2),IR_READ_PER_BEAC)][loopCounter]; |
| re633 | 9:ef0907fda2f1 | 143 | gv_IRValDiffs[gv_counter25ms][loopCounter] = IRchange; |
| homero | 13:c18d82f62d38 | 144 | gv_IRValDiffsTwo[gv_counter25ms][loopCounter] = IRchange2; |
| homero | 13:c18d82f62d38 | 145 | |
| re633 | 9:ef0907fda2f1 | 146 | //printf("change %d count %d\n",IRchange); |
| re633 | 9:ef0907fda2f1 | 147 | //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 | 148 | if (IRchange > BEACON_SUSPECTED){ |
| re633 | 11:c5094a68283f | 149 | tick_beacon_suspected = gv_counter25ms; |
| re633 | 9:ef0907fda2f1 | 150 | piswarm.cls(); |
| re633 | 11:c5094a68283f | 151 | piswarm.printf("%d",tick_beacon_suspected); |
| re633 | 9:ef0907fda2f1 | 152 | } |
| re633 | 9:ef0907fda2f1 | 153 | } |
| homero | 13:c18d82f62d38 | 154 | |
| re633 | 9:ef0907fda2f1 | 155 | //Now store the illuminated values if the beacon is not illuminated- |
| re633 | 9:ef0907fda2f1 | 156 | piswarm.store_illuminated_raw_ir_values(); |
| homero | 13:c18d82f62d38 | 157 | |
| re633 | 9:ef0907fda2f1 | 158 | //In this loop convert each raw active IR reading into a distance estimate |
| re633 | 9:ef0907fda2f1 | 159 | for(loopCounter = 0; loopCounter < 8; loopCounter++) { |
| re633 | 9:ef0907fda2f1 | 160 | |
| re633 | 9:ef0907fda2f1 | 161 | //Specific sensor readings converted to distances |
| re633 | 9:ef0907fda2f1 | 162 | float temp = piswarm.get_illuminated_raw_ir_value(loopCounter); |
| homero | 13:c18d82f62d38 | 163 | //if(gv_counter25ms == 1) pc.printf("sen %d :%f\n", loopCounter,temp); |
| homero | 13:c18d82f62d38 | 164 | if(gv_counter25ms == 0){ |
| homero | 13:c18d82f62d38 | 165 | pc.printf("sen %d raw: %f",loopCounter,temp); |
| homero | 13:c18d82f62d38 | 166 | } |
| re633 | 9:ef0907fda2f1 | 167 | if(temp>3500){ |
| re633 | 9:ef0907fda2f1 | 168 | temp = 3500; |
| re633 | 9:ef0907fda2f1 | 169 | } else if (temp < 97){ |
| re633 | 9:ef0907fda2f1 | 170 | temp = 97; |
| re633 | 9:ef0907fda2f1 | 171 | } |
| re633 | 9:ef0907fda2f1 | 172 | //#put this into a function |
| re633 | 9:ef0907fda2f1 | 173 | //Switch case for robot 5 |
| homero | 13:c18d82f62d38 | 174 | switch(robot_id){ |
| homero | 13:c18d82f62d38 | 175 | case 9: |
| homero | 13:c18d82f62d38 | 176 | switch(loopCounter){ |
| homero | 13:c18d82f62d38 | 177 | case 0: |
| homero | 13:c18d82f62d38 | 178 | temp = 1643 * sqrt(1/(temp-190))-35; |
| homero | 13:c18d82f62d38 | 179 | break; |
| homero | 13:c18d82f62d38 | 180 | case 1: |
| homero | 13:c18d82f62d38 | 181 | temp = 1097 * sqrt(1/(temp-190))-24; |
| homero | 13:c18d82f62d38 | 182 | break; |
| homero | 13:c18d82f62d38 | 183 | case 2: |
| homero | 13:c18d82f62d38 | 184 | temp = 838 * sqrt(1/(temp-120))-17; |
| homero | 13:c18d82f62d38 | 185 | break; |
| homero | 13:c18d82f62d38 | 186 | case 3: |
| homero | 13:c18d82f62d38 | 187 | temp = 1017 * sqrt(1/(temp-175))-22; |
| homero | 13:c18d82f62d38 | 188 | break; |
| homero | 13:c18d82f62d38 | 189 | case 4: |
| homero | 13:c18d82f62d38 | 190 | temp = 777 * sqrt(1/(temp-130))-16; |
| homero | 13:c18d82f62d38 | 191 | break; |
| homero | 13:c18d82f62d38 | 192 | case 5: |
| homero | 13:c18d82f62d38 | 193 | temp = 765 * sqrt(1/(temp-115))-11; |
| homero | 13:c18d82f62d38 | 194 | break; |
| homero | 13:c18d82f62d38 | 195 | case 6: |
| homero | 13:c18d82f62d38 | 196 | temp = 1086 * sqrt(1/(temp-150))-17; |
| homero | 13:c18d82f62d38 | 197 | break; |
| homero | 13:c18d82f62d38 | 198 | case 7: |
| homero | 13:c18d82f62d38 | 199 | temp = 1578 * sqrt(1/(temp-220))-24; |
| homero | 13:c18d82f62d38 | 200 | break; |
| homero | 13:c18d82f62d38 | 201 | } |
| re633 | 9:ef0907fda2f1 | 202 | break; |
| homero | 13:c18d82f62d38 | 203 | case 12: |
| homero | 13:c18d82f62d38 | 204 | switch(loopCounter){ |
| homero | 13:c18d82f62d38 | 205 | case 0: |
| homero | 13:c18d82f62d38 | 206 | temp = 877 * sqrt(1/(temp-80))-13; |
| homero | 13:c18d82f62d38 | 207 | break; |
| homero | 13:c18d82f62d38 | 208 | case 1: |
| homero | 13:c18d82f62d38 | 209 | temp = 887 * sqrt(1/(temp-110))-13; |
| homero | 13:c18d82f62d38 | 210 | break; |
| homero | 13:c18d82f62d38 | 211 | case 2: |
| homero | 13:c18d82f62d38 | 212 | temp = 744 * sqrt(1/(temp-90))-8; |
| homero | 13:c18d82f62d38 | 213 | break; |
| homero | 13:c18d82f62d38 | 214 | case 3: |
| homero | 13:c18d82f62d38 | 215 | temp = 816 * sqrt(1/(temp-105))-17; |
| homero | 13:c18d82f62d38 | 216 | break; |
| homero | 13:c18d82f62d38 | 217 | case 4: |
| homero | 13:c18d82f62d38 | 218 | temp = 821 * sqrt(1/(temp-125))-7; |
| homero | 13:c18d82f62d38 | 219 | break; |
| homero | 13:c18d82f62d38 | 220 | case 5: |
| homero | 13:c18d82f62d38 | 221 | temp = 741 * sqrt(1/(temp-110))-7; |
| homero | 13:c18d82f62d38 | 222 | break; |
| homero | 13:c18d82f62d38 | 223 | case 6: |
| homero | 13:c18d82f62d38 | 224 | temp = 1000 * sqrt(1/(temp-95))-18; |
| homero | 13:c18d82f62d38 | 225 | break; |
| homero | 13:c18d82f62d38 | 226 | case 7: |
| homero | 13:c18d82f62d38 | 227 | temp = 924 * sqrt(1/(temp-115))-12; |
| homero | 13:c18d82f62d38 | 228 | break; |
| homero | 13:c18d82f62d38 | 229 | } |
| re633 | 9:ef0907fda2f1 | 230 | break; |
| homero | 13:c18d82f62d38 | 231 | default: |
| homero | 13:c18d82f62d38 | 232 | temp = 662 * sqrt(1/(temp-152)); |
| homero | 13:c18d82f62d38 | 233 | |
| re633 | 9:ef0907fda2f1 | 234 | } |
| homero | 13:c18d82f62d38 | 235 | |
| homero | 13:c18d82f62d38 | 236 | if(gv_counter25ms == 0){ |
| homero | 13:c18d82f62d38 | 237 | pc.printf("sen %d dist:%f\n",loopCounter,temp); |
| homero | 13:c18d82f62d38 | 238 | } |
| re633 | 9:ef0907fda2f1 | 239 | if (temp > 130){ |
| re633 | 9:ef0907fda2f1 | 240 | temp = 130; |
| re633 | 9:ef0907fda2f1 | 241 | } |
| re633 | 9:ef0907fda2f1 | 242 | gv_IRDistances[loopCounter] = temp; |
| re633 | 9:ef0907fda2f1 | 243 | |
| homero | 13:c18d82f62d38 | 244 | } |
| homero | 13:c18d82f62d38 | 245 | |
| re633 | 9:ef0907fda2f1 | 246 | //reset counter after 1 second (beacon period) |
| re633 | 9:ef0907fda2f1 | 247 | gv_counter25ms = mod8(gv_counter25ms + 1,IR_READ_PER_BEAC); |
| re633 | 9:ef0907fda2f1 | 248 | } |
| re633 | 9:ef0907fda2f1 | 249 | |
| homero | 13:c18d82f62d38 | 250 | //Get the distance of the ultrsonic sensor in cm |
| homero | 13:c18d82f62d38 | 251 | void get_ultrasonic_readings(){ |
| homero | 13:c18d82f62d38 | 252 | float old_dist = distance_ultrasonic_sensor; |
| homero | 13:c18d82f62d38 | 253 | int static count = 0; |
| homero | 13:c18d82f62d38 | 254 | distance_ultrasonic_sensor = usensor.get_dist_mm(); |
| homero | 13:c18d82f62d38 | 255 | if(count <100){ |
| homero | 13:c18d82f62d38 | 256 | pc.printf("US: %.1f, %d\n",distance_ultrasonic_sensor, count); |
| homero | 13:c18d82f62d38 | 257 | count ++; |
| homero | 13:c18d82f62d38 | 258 | }else{ |
| homero | 13:c18d82f62d38 | 259 | count =0; |
| homero | 13:c18d82f62d38 | 260 | } |
| homero | 13:c18d82f62d38 | 261 | |
| homero | 13:c18d82f62d38 | 262 | if(distance_ultrasonic_sensor <= 0 || distance_ultrasonic_sensor == 1000){ |
| homero | 13:c18d82f62d38 | 263 | distance_ultrasonic_sensor = old_dist; |
| homero | 13:c18d82f62d38 | 264 | } |
| homero | 13:c18d82f62d38 | 265 | //piswarm.cls(); |
| homero | 13:c18d82f62d38 | 266 | |
| homero | 13:c18d82f62d38 | 267 | usensor.start(); |
| homero | 13:c18d82f62d38 | 268 | } |
| homero | 13:c18d82f62d38 | 269 | |
| homero | 13:c18d82f62d38 | 270 | //When called the readIRs ticker will be reattached after the specified time. |
| homero | 13:c18d82f62d38 | 271 | void atTimeout(){ |
| homero | 13:c18d82f62d38 | 272 | ticker_25ms.attach_us(&readIRs,25000); |
| homero | 13:c18d82f62d38 | 273 | } |
| homero | 13:c18d82f62d38 | 274 | |
| homero | 13:c18d82f62d38 | 275 | void atBroadcastTimeout(){ |
| homero | 13:c18d82f62d38 | 276 | char function; |
| homero | 13:c18d82f62d38 | 277 | if(robot_id == 1|| robot_id == 12){ |
| homero | 13:c18d82f62d38 | 278 | function = 2; |
| homero | 13:c18d82f62d38 | 279 | } else if(robot_id == 9){ |
| homero | 13:c18d82f62d38 | 280 | function = 3; |
| homero | 13:c18d82f62d38 | 281 | } |
| homero | 13:c18d82f62d38 | 282 | |
| homero | 13:c18d82f62d38 | 283 | char message[2]; |
| homero | 13:c18d82f62d38 | 284 | message[0] = piswarm.get_id()+48; |
| homero | 13:c18d82f62d38 | 285 | broadcast_user_rf_command(function,message,1); |
| homero | 13:c18d82f62d38 | 286 | broadcasted_count++; |
| homero | 13:c18d82f62d38 | 287 | broadcasted_flag = 1; |
| homero | 13:c18d82f62d38 | 288 | } |
| homero | 13:c18d82f62d38 | 289 | //This is a wait function for one |
| homero | 13:c18d82f62d38 | 290 | |
| re633 | 9:ef0907fda2f1 | 291 | //******************************************************************************************************* |
| jah128 | 8:a789ef4fde52 | 292 | //This is where the program code goes. |
| jah128 | 1:37502eb3b70f | 293 | int main() { |
| re633 | 9:ef0907fda2f1 | 294 | init(); |
| homero | 13:c18d82f62d38 | 295 | robot_id = piswarm.get_id(); |
| re633 | 9:ef0907fda2f1 | 296 | //starting point in state 11 |
| homero | 13:c18d82f62d38 | 297 | //usensor.start(); // get first reading of the ultrasonic sensor required before ticker |
| homero | 13:c18d82f62d38 | 298 | //wait_ms(50); |
| re633 | 9:ef0907fda2f1 | 299 | |
| homero | 13:c18d82f62d38 | 300 | |
| homero | 13:c18d82f62d38 | 301 | //wait(1); //Wait a second to allow IR array to be filled |
| jah128 | 8:a789ef4fde52 | 302 | |
| re633 | 9:ef0907fda2f1 | 303 | //Controller is a finite state machine |
| re633 | 9:ef0907fda2f1 | 304 | while(1){ |
| re633 | 9:ef0907fda2f1 | 305 | |
| re633 | 9:ef0907fda2f1 | 306 | //Waiting for signal to begin searching |
| homero | 13:c18d82f62d38 | 307 | if(gv_state == States::READY_TO_START){ |
| homero | 13:c18d82f62d38 | 308 | //pc.printf("%d\n",start_flag); |
| re633 | 12:118f2b0ed8eb | 309 | if(start_flag == 1){ |
| re633 | 12:118f2b0ed8eb | 310 | //Change state here after recieving a radio command |
| homero | 13:c18d82f62d38 | 311 | ticker_25ms.attach_us(&readIRs,25000); |
| homero | 13:c18d82f62d38 | 312 | |
| homero | 13:c18d82f62d38 | 313 | if(robot_id == 1){ |
| homero | 14:fc406dfff94f | 314 | at_beacon_threshold = 3850; |
| homero | 13:c18d82f62d38 | 315 | } |
| homero | 13:c18d82f62d38 | 316 | |
| homero | 13:c18d82f62d38 | 317 | else if (robot_id == 9){ |
| homero | 14:fc406dfff94f | 318 | at_beacon_threshold = 3950; |
| homero | 13:c18d82f62d38 | 319 | } |
| homero | 13:c18d82f62d38 | 320 | |
| homero | 13:c18d82f62d38 | 321 | else if (robot_id == 12){ |
| homero | 14:fc406dfff94f | 322 | at_beacon_threshold = 3980; |
| homero | 13:c18d82f62d38 | 323 | r_mot_scaler = 1.02; |
| homero | 13:c18d82f62d38 | 324 | l_mot_scaler = 0.98; |
| homero | 13:c18d82f62d38 | 325 | } |
| homero | 13:c18d82f62d38 | 326 | |
| homero | 13:c18d82f62d38 | 327 | else{ |
| homero | 14:fc406dfff94f | 328 | at_beacon_threshold = 3900; |
| homero | 13:c18d82f62d38 | 329 | } |
| homero | 13:c18d82f62d38 | 330 | |
| homero | 13:c18d82f62d38 | 331 | usensor.start(); // get first reading of the ultrasonic sensor required before ticker |
| homero | 13:c18d82f62d38 | 332 | wait_ms(50); |
| homero | 13:c18d82f62d38 | 333 | ticker_ultrasonic50ms.attach_us(&get_ultrasonic_readings,50000); |
| homero | 13:c18d82f62d38 | 334 | |
| homero | 14:fc406dfff94f | 335 | turn_timer.start(); |
| homero | 14:fc406dfff94f | 336 | levy_timer.start(); |
| homero | 13:c18d82f62d38 | 337 | //pc.printf("why\n"); |
| homero | 13:c18d82f62d38 | 338 | changeState(States::SEARCHING_FWD); |
| re633 | 10:da62735d6df9 | 339 | } |
| homero | 13:c18d82f62d38 | 340 | |
| re633 | 9:ef0907fda2f1 | 341 | //Searching state |
| homero | 14:fc406dfff94f | 342 | } else if (gv_state == States::SEARCHING_FWD || gv_state == States::SEARCHING_TURNING){ |
| re633 | 9:ef0907fda2f1 | 343 | |
| re633 | 12:118f2b0ed8eb | 344 | //Do something here on receipt of 'function 5' if necessary. |
| re633 | 12:118f2b0ed8eb | 345 | //As currently the home beacon will immediately switch on that is not necessary. |
| re633 | 12:118f2b0ed8eb | 346 | |
| re633 | 12:118f2b0ed8eb | 347 | //Determine if suspected beacon is actually the beacon. |
| re633 | 9:ef0907fda2f1 | 348 | //This is done by checking the period between flashes matches the beacon period |
| re633 | 11:c5094a68283f | 349 | if(tick_beacon_suspected != 100){ |
| re633 | 9:ef0907fda2f1 | 350 | //When the beacon flag is first raised store its value and reset it |
| re633 | 11:c5094a68283f | 351 | if(tick_beacon_period_check == 100){ |
| re633 | 11:c5094a68283f | 352 | tick_beacon_period_check = tick_beacon_suspected; |
| re633 | 11:c5094a68283f | 353 | tick_beacon_suspected = 100; |
| re633 | 9:ef0907fda2f1 | 354 | //Check the timing of the latest jump with the last one to see if period matches the Beacon. |
| re633 | 9:ef0907fda2f1 | 355 | } else { |
| re633 | 9:ef0907fda2f1 | 356 | piswarm.locate(0,1); |
| re633 | 11:c5094a68283f | 357 | piswarm.printf("%d %d",tick_beacon_period_check,tick_beacon_suspected); |
| re633 | 11:c5094a68283f | 358 | //printf("%d %d *********************************",tick_beacon_period_check,tick_beacon_suspected); |
| re633 | 9:ef0907fda2f1 | 359 | //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 | 360 | int8_t test = (tick_beacon_period_check - tick_beacon_suspected); |
| re633 | 9:ef0907fda2f1 | 361 | |
| re633 | 9:ef0907fda2f1 | 362 | test = test * test; |
| re633 | 9:ef0907fda2f1 | 363 | |
| re633 | 9:ef0907fda2f1 | 364 | //if test is low then identify the beacon as the cause of the flags |
| re633 | 9:ef0907fda2f1 | 365 | if(test < 2){ |
| re633 | 9:ef0907fda2f1 | 366 | //Beacon found change to state 2 |
| re633 | 11:c5094a68283f | 367 | g_beaconOn = tick_beacon_period_check; //update the global variable that stores when beacon flashes occur |
| re633 | 9:ef0907fda2f1 | 368 | |
| homero | 13:c18d82f62d38 | 369 | //wait(2); |
| homero | 13:c18d82f62d38 | 370 | changeState(States::MOVING_TO_BEACON); |
| re633 | 9:ef0907fda2f1 | 371 | } else { |
| re633 | 9:ef0907fda2f1 | 372 | //Reset the flag to try again |
| re633 | 11:c5094a68283f | 373 | tick_beacon_period_check = 100; |
| re633 | 9:ef0907fda2f1 | 374 | } |
| re633 | 9:ef0907fda2f1 | 375 | } |
| re633 | 9:ef0907fda2f1 | 376 | } |
| re633 | 9:ef0907fda2f1 | 377 | |
| homero | 13:c18d82f62d38 | 378 | if(gv_state == States::SEARCHING_FWD){ |
| homero | 14:fc406dfff94f | 379 | |
| homero | 13:c18d82f62d38 | 380 | |
| re633 | 9:ef0907fda2f1 | 381 | if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){ |
| re633 | 9:ef0907fda2f1 | 382 | piswarm.stop(); |
| re633 | 10:da62735d6df9 | 383 | piswarm.cls(); |
| re633 | 10:da62735d6df9 | 384 | piswarm.printf("ob R"); |
| re633 | 10:da62735d6df9 | 385 | piswarm.play_tune("CC",1); |
| homero | 14:fc406dfff94f | 386 | g_obstacle_type = 1; |
| homero | 13:c18d82f62d38 | 387 | changeState(States::SEARCHING_TURNING); |
| homero | 13:c18d82f62d38 | 388 | |
| homero | 13:c18d82f62d38 | 389 | //Avoid obstacle to the left |
| re633 | 9:ef0907fda2f1 | 390 | } else if(gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){ |
| re633 | 9:ef0907fda2f1 | 391 | piswarm.stop(); |
| re633 | 10:da62735d6df9 | 392 | piswarm.cls(); |
| re633 | 10:da62735d6df9 | 393 | piswarm.printf("ob L"); |
| re633 | 10:da62735d6df9 | 394 | piswarm.play_tune("CC",1); |
| homero | 14:fc406dfff94f | 395 | g_obstacle_type = 2; |
| homero | 13:c18d82f62d38 | 396 | changeState(States::SEARCHING_TURNING); |
| homero | 13:c18d82f62d38 | 397 | |
| homero | 14:fc406dfff94f | 398 | } else if(distance_ultrasonic_sensor < 70){ |
| homero | 13:c18d82f62d38 | 399 | piswarm.stop(); |
| homero | 13:c18d82f62d38 | 400 | piswarm.cls(); |
| homero | 13:c18d82f62d38 | 401 | piswarm.printf("ob F"); |
| homero | 13:c18d82f62d38 | 402 | piswarm.play_tune("DD",1); |
| homero | 13:c18d82f62d38 | 403 | //wait(0.1); |
| homero | 14:fc406dfff94f | 404 | g_obstacle_type = 3; |
| homero | 13:c18d82f62d38 | 405 | changeState(States::SEARCHING_TURNING); |
| homero | 13:c18d82f62d38 | 406 | |
| homero | 13:c18d82f62d38 | 407 | |
| homero | 13:c18d82f62d38 | 408 | } else if(distance_ultrasonic_sensor <= 80){ |
| homero | 14:fc406dfff94f | 409 | g_obstacle_type = 3; |
| homero | 13:c18d82f62d38 | 410 | changeState(States::SEARCHING_TURNING); |
| homero | 13:c18d82f62d38 | 411 | |
| homero | 14:fc406dfff94f | 412 | } else if(levy_timer.read_us() > levy_target_time_us){ |
| homero | 14:fc406dfff94f | 413 | g_back_count++; |
| homero | 14:fc406dfff94f | 414 | set_new_levy_time_flag = 1; |
| re633 | 10:da62735d6df9 | 415 | piswarm.play_tune("G",1); |
| homero | 13:c18d82f62d38 | 416 | piswarm.set_oled_colour(255,0,0); |
| homero | 13:c18d82f62d38 | 417 | changeState(States::SEARCHING_TURNING); |
| homero | 14:fc406dfff94f | 418 | }else if(g_back_count >=8){ |
| homero | 14:fc406dfff94f | 419 | g_back_count = 0; |
| homero | 13:c18d82f62d38 | 420 | piswarm.stop(); |
| homero | 13:c18d82f62d38 | 421 | wait_ms(200); |
| homero | 13:c18d82f62d38 | 422 | piswarm.left_motor(-0.4*l_mot_scaler); |
| homero | 13:c18d82f62d38 | 423 | piswarm.right_motor(-0.4*r_mot_scaler); |
| homero | 13:c18d82f62d38 | 424 | wait(0.5); |
| homero | 13:c18d82f62d38 | 425 | changeState(States::SEARCHING_TURNING); |
| homero | 13:c18d82f62d38 | 426 | |
| homero | 13:c18d82f62d38 | 427 | } else if (distance_ultrasonic_sensor <= 800){ |
| homero | 13:c18d82f62d38 | 428 | //decrease speed when PiSwarm is getting close to an obstacle |
| homero | 13:c18d82f62d38 | 429 | float velocity = 0.2; |
| homero | 13:c18d82f62d38 | 430 | if(distance_ultrasonic_sensor > 100){ |
| homero | 13:c18d82f62d38 | 431 | velocity = (distance_ultrasonic_sensor*0.00057) + 0.142; |
| homero | 13:c18d82f62d38 | 432 | } |
| homero | 13:c18d82f62d38 | 433 | piswarm.left_motor(velocity*l_mot_scaler); |
| homero | 13:c18d82f62d38 | 434 | piswarm.right_motor(velocity*r_mot_scaler); |
| homero | 13:c18d82f62d38 | 435 | //wait_ms(0.2); |
| homero | 13:c18d82f62d38 | 436 | |
| homero | 13:c18d82f62d38 | 437 | |
| homero | 13:c18d82f62d38 | 438 | //Otherwise continue moving forward until distance determined by levy algorithm is calculated. |
| homero | 13:c18d82f62d38 | 439 | |
| re633 | 9:ef0907fda2f1 | 440 | |
| homero | 13:c18d82f62d38 | 441 | } else { |
| homero | 13:c18d82f62d38 | 442 | piswarm.right_motor(BASE_SPEED*r_mot_scaler); |
| homero | 13:c18d82f62d38 | 443 | piswarm.left_motor(BASE_SPEED*l_mot_scaler); |
| homero | 13:c18d82f62d38 | 444 | } |
| homero | 13:c18d82f62d38 | 445 | //wait to get new ultrasound reading |
| homero | 13:c18d82f62d38 | 446 | //wait_ms(50); |
| re633 | 9:ef0907fda2f1 | 447 | |
| homero | 13:c18d82f62d38 | 448 | |
| homero | 13:c18d82f62d38 | 449 | } else if(gv_state == States::SEARCHING_TURNING){ |
| re633 | 9:ef0907fda2f1 | 450 | piswarm.stop();//Stop the robot. |
| re633 | 9:ef0907fda2f1 | 451 | int16_t randomAngle; |
| re633 | 9:ef0907fda2f1 | 452 | //If sent here beacuse of obstacle find angle between -180 to -90 and 90 to 180 |
| homero | 14:fc406dfff94f | 453 | if(g_obstacle_type == 1){ |
| re633 | 10:da62735d6df9 | 454 | randomAngle = rand()%90 - 135; |
| re633 | 9:ef0907fda2f1 | 455 | |
| homero | 14:fc406dfff94f | 456 | } else if(g_obstacle_type == 2){ |
| re633 | 10:da62735d6df9 | 457 | randomAngle = rand()%90 + 45; |
| re633 | 10:da62735d6df9 | 458 | |
| homero | 14:fc406dfff94f | 459 | } else if(g_obstacle_type == 3){ |
| homero | 14:fc406dfff94f | 460 | randomAngle = rand()%90 + 45; |
| homero | 13:c18d82f62d38 | 461 | |
| re633 | 10:da62735d6df9 | 462 | //Otherwise if here due to levy walk: turn to any random angle |
| re633 | 9:ef0907fda2f1 | 463 | } else { |
| re633 | 10:da62735d6df9 | 464 | randomAngle = rand()%360 - 180; |
| re633 | 9:ef0907fda2f1 | 465 | } |
| re633 | 9:ef0907fda2f1 | 466 | turnDegrees(randomAngle); //Make the turn |
| homero | 14:fc406dfff94f | 467 | g_obstacle_type = 0; |
| homero | 13:c18d82f62d38 | 468 | changeState(States::SEARCHING_FWD);//Move back into state 11 |
| homero | 13:c18d82f62d38 | 469 | |
| re633 | 9:ef0907fda2f1 | 470 | //Beacon found state |
| homero | 13:c18d82f62d38 | 471 | } else if (gv_state == States::MOVING_TO_BEACON){ |
| re633 | 12:118f2b0ed8eb | 472 | |
| re633 | 12:118f2b0ed8eb | 473 | //Do something here on receipt of 'function 5' if necessary. |
| re633 | 12:118f2b0ed8eb | 474 | //As currently the home beacon will immediately switch on that is not necessary. |
| re633 | 12:118f2b0ed8eb | 475 | |
| homero | 13:c18d82f62d38 | 476 | static int16_t valid_distances[8] = {0}; |
| homero | 13:c18d82f62d38 | 477 | |
| re633 | 9:ef0907fda2f1 | 478 | int16_t maxValue[2] = {0,100}; //Value and sensor position |
| homero | 13:c18d82f62d38 | 479 | int8_t loopCounter = 0; |
| homero | 13:c18d82f62d38 | 480 | |
| homero | 14:fc406dfff94f | 481 | if(beacon_illuminated_flag == 0){ |
| homero | 13:c18d82f62d38 | 482 | for(loopCounter = 0; loopCounter < 8; loopCounter++){ |
| homero | 13:c18d82f62d38 | 483 | valid_distances[loopCounter] = gv_IRDistances[loopCounter]; |
| homero | 13:c18d82f62d38 | 484 | } |
| homero | 13:c18d82f62d38 | 485 | } |
| re633 | 9:ef0907fda2f1 | 486 | |
| re633 | 9:ef0907fda2f1 | 487 | //If beacon visible |
| homero | 14:fc406dfff94f | 488 | if(beacon_syncronised_flag == 1){ |
| re633 | 9:ef0907fda2f1 | 489 | |
| re633 | 9:ef0907fda2f1 | 490 | //Firstly check beacon is still visible |
| homero | 14:fc406dfff94f | 491 | beacon_syncronised_flag = 0; |
| re633 | 9:ef0907fda2f1 | 492 | //Update array concerning which IRs can see the beacon |
| re633 | 9:ef0907fda2f1 | 493 | for(loopCounter = 0; loopCounter<8; loopCounter++) { |
| re633 | 9:ef0907fda2f1 | 494 | |
| re633 | 9:ef0907fda2f1 | 495 | //Find which sensor has the highest reading |
| re633 | 9:ef0907fda2f1 | 496 | if( gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED) { |
| homero | 13:c18d82f62d38 | 497 | if( valid_distances[loopCounter] > 100 && valid_distances[mod8((loopCounter + 1),8)]>100 && valid_distances[mod8((loopCounter - 1),8)]>100){ |
| homero | 13:c18d82f62d38 | 498 | if(gv_IRVals[g_beaconOn][loopCounter] > maxValue[0]){ |
| homero | 13:c18d82f62d38 | 499 | maxValue[0] = gv_IRVals[g_beaconOn][loopCounter]; |
| homero | 13:c18d82f62d38 | 500 | maxValue[1] = loopCounter; |
| homero | 14:fc406dfff94f | 501 | beacon_syncronised_flag = 1; //This will remain as one so long as at least on sensor can see beacon |
| homero | 13:c18d82f62d38 | 502 | } |
| re633 | 9:ef0907fda2f1 | 503 | } |
| re633 | 9:ef0907fda2f1 | 504 | } |
| re633 | 9:ef0907fda2f1 | 505 | } |
| re633 | 9:ef0907fda2f1 | 506 | |
| re633 | 9:ef0907fda2f1 | 507 | //Only do this if beacon still visible |
| homero | 14:fc406dfff94f | 508 | if(beacon_syncronised_flag == 1){ |
| re633 | 9:ef0907fda2f1 | 509 | |
| re633 | 9:ef0907fda2f1 | 510 | //If the adjacent two sensors are above the threshold too then they can also be marked as illuminated |
| re633 | 9:ef0907fda2f1 | 511 | for(loopCounter = 0; loopCounter<8; loopCounter++){ |
| re633 | 9:ef0907fda2f1 | 512 | |
| re633 | 9:ef0907fda2f1 | 513 | //reset all beacon detected values |
| re633 | 9:ef0907fda2f1 | 514 | beacon_detected[loopCounter] = 0; |
| re633 | 9:ef0907fda2f1 | 515 | |
| re633 | 9:ef0907fda2f1 | 516 | if(abs(maxValue[1] - loopCounter)< 3 || abs(maxValue[1] + 8 - loopCounter)< 3 || abs(maxValue[1] - 8 - loopCounter)< 3) { |
| re633 | 9:ef0907fda2f1 | 517 | if(gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED){ |
| re633 | 9:ef0907fda2f1 | 518 | beacon_detected[loopCounter] = 1; |
| re633 | 9:ef0907fda2f1 | 519 | } |
| re633 | 9:ef0907fda2f1 | 520 | } |
| re633 | 9:ef0907fda2f1 | 521 | } |
| re633 | 9:ef0907fda2f1 | 522 | |
| re633 | 9:ef0907fda2f1 | 523 | |
| re633 | 9:ef0907fda2f1 | 524 | //Update the piswarm LEDS so the ones that can see the beacon are on. |
| re633 | 9:ef0907fda2f1 | 525 | piswarm.set_oleds(beacon_detected[0]||beacon_detected[1], |
| re633 | 9:ef0907fda2f1 | 526 | beacon_detected[1]||beacon_detected[2], |
| re633 | 9:ef0907fda2f1 | 527 | beacon_detected[2], |
| re633 | 9:ef0907fda2f1 | 528 | beacon_detected[3], |
| re633 | 9:ef0907fda2f1 | 529 | 0, |
| re633 | 9:ef0907fda2f1 | 530 | beacon_detected[4], |
| re633 | 9:ef0907fda2f1 | 531 | beacon_detected[5], |
| re633 | 9:ef0907fda2f1 | 532 | beacon_detected[5]||beacon_detected[6], |
| re633 | 9:ef0907fda2f1 | 533 | beacon_detected[6]||beacon_detected[7], |
| re633 | 9:ef0907fda2f1 | 534 | beacon_detected[7]||beacon_detected[0]); |
| re633 | 9:ef0907fda2f1 | 535 | |
| re633 | 9:ef0907fda2f1 | 536 | //If the max IR value is below a threshold then move toward beacon. Else change state |
| homero | 14:fc406dfff94f | 537 | if(maxValue[0] < at_beacon_threshold){ |
| re633 | 9:ef0907fda2f1 | 538 | |
| re633 | 9:ef0907fda2f1 | 539 | //Calculate the heading of Pi-Swarm Relative to beacon |
| re633 | 9:ef0907fda2f1 | 540 | calculateNewHeading(); |
| homero | 14:fc406dfff94f | 541 | |
| homero | 13:c18d82f62d38 | 542 | if(g_currentHeading > 5 || g_currentHeading < -5){ |
| re633 | 9:ef0907fda2f1 | 543 | turnDegrees(-g_currentHeading); |
| re633 | 9:ef0907fda2f1 | 544 | } |
| re633 | 9:ef0907fda2f1 | 545 | |
| re633 | 9:ef0907fda2f1 | 546 | //If the beacon is not currently on but obstacle detected then do obstacle avoidance |
| homero | 13:c18d82f62d38 | 547 | int16_t randomAngle; |
| homero | 14:fc406dfff94f | 548 | if(beacon_illuminated_flag == 0){ |
| re633 | 9:ef0907fda2f1 | 549 | if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){ |
| homero | 13:c18d82f62d38 | 550 | randomAngle = rand()%90 - 135; |
| homero | 13:c18d82f62d38 | 551 | piswarm.stop(); |
| homero | 13:c18d82f62d38 | 552 | wait_ms(100); |
| homero | 13:c18d82f62d38 | 553 | piswarm.backward(0.3); |
| homero | 13:c18d82f62d38 | 554 | wait_ms(200); |
| homero | 13:c18d82f62d38 | 555 | turnDegrees(randomAngle); |
| re633 | 9:ef0907fda2f1 | 556 | } else if (gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){ |
| homero | 13:c18d82f62d38 | 557 | randomAngle = rand()%90 + 45; |
| homero | 13:c18d82f62d38 | 558 | piswarm.stop(); |
| homero | 13:c18d82f62d38 | 559 | wait_ms(100); |
| homero | 13:c18d82f62d38 | 560 | piswarm.backward(0.3); |
| homero | 13:c18d82f62d38 | 561 | wait_ms(200); |
| homero | 13:c18d82f62d38 | 562 | turnDegrees(randomAngle); |
| homero | 13:c18d82f62d38 | 563 | } else if ( distance_ultrasonic_sensor < 100){ |
| homero | 13:c18d82f62d38 | 564 | randomAngle = rand()%60 - 30; |
| homero | 13:c18d82f62d38 | 565 | piswarm.stop(); |
| homero | 13:c18d82f62d38 | 566 | wait_ms(100); |
| homero | 13:c18d82f62d38 | 567 | piswarm.backward(0.3); |
| homero | 13:c18d82f62d38 | 568 | wait_ms(200); |
| homero | 13:c18d82f62d38 | 569 | turnDegrees(randomAngle); |
| re633 | 9:ef0907fda2f1 | 570 | } |
| re633 | 9:ef0907fda2f1 | 571 | } |
| homero | 14:fc406dfff94f | 572 | piswarm.right_motor(0.3*r_mot_scaler*r_mot_scaler); |
| homero | 14:fc406dfff94f | 573 | piswarm.left_motor(0.3*l_mot_scaler*l_mot_scaler); |
| homero | 14:fc406dfff94f | 574 | wait_ms(500); |
| re633 | 9:ef0907fda2f1 | 575 | //Should be at beacon |
| re633 | 9:ef0907fda2f1 | 576 | } else { |
| re633 | 9:ef0907fda2f1 | 577 | piswarm.stop(); |
| homero | 13:c18d82f62d38 | 578 | calculateNewHeading(); |
| homero | 13:c18d82f62d38 | 579 | if(g_currentHeading > 5 || g_currentHeading < -5){ |
| homero | 13:c18d82f62d38 | 580 | turnDegrees(-g_currentHeading); |
| homero | 13:c18d82f62d38 | 581 | } |
| re633 | 9:ef0907fda2f1 | 582 | |
| re633 | 12:118f2b0ed8eb | 583 | //If either of these flags is one then the beacon should be the home beacon so change to state 4. |
| re633 | 12:118f2b0ed8eb | 584 | if(return_flag == 1 || back_flag == 1){ |
| homero | 13:c18d82f62d38 | 585 | changeState(States::AT_HOME_BEACON); |
| re633 | 12:118f2b0ed8eb | 586 | } else { |
| homero | 13:c18d82f62d38 | 587 | changeState(States::AT_TARGET_BEACON); |
| re633 | 12:118f2b0ed8eb | 588 | } |
| re633 | 9:ef0907fda2f1 | 589 | } |
| re633 | 9:ef0907fda2f1 | 590 | } |
| re633 | 9:ef0907fda2f1 | 591 | //Else need to syncronise with beacon |
| re633 | 9:ef0907fda2f1 | 592 | } else { |
| re633 | 9:ef0907fda2f1 | 593 | |
| homero | 14:fc406dfff94f | 594 | while(beacon_syncronised_flag == 0){ |
| homero | 14:fc406dfff94f | 595 | |
| re633 | 9:ef0907fda2f1 | 596 | //Sychronise the ticker with the beacon |
| re633 | 9:ef0907fda2f1 | 597 | uint8_t testBefore = 0; |
| re633 | 9:ef0907fda2f1 | 598 | uint8_t testDuring = 0; |
| re633 | 9:ef0907fda2f1 | 599 | uint8_t testAfter = 0; |
| re633 | 9:ef0907fda2f1 | 600 | for(loopCounter = 0; loopCounter < 8; loopCounter++){ |
| re633 | 9:ef0907fda2f1 | 601 | if (gv_IRValDiffs[mod8((g_beaconOn - 1),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){ |
| re633 | 9:ef0907fda2f1 | 602 | testBefore = 1; |
| re633 | 9:ef0907fda2f1 | 603 | } |
| re633 | 9:ef0907fda2f1 | 604 | if (gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED){ |
| re633 | 9:ef0907fda2f1 | 605 | testDuring = 1; |
| re633 | 9:ef0907fda2f1 | 606 | } |
| homero | 13:c18d82f62d38 | 607 | if (gv_IRValDiffsTwo[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){ |
| re633 | 9:ef0907fda2f1 | 608 | testAfter = 1; |
| re633 | 9:ef0907fda2f1 | 609 | } |
| homero | 13:c18d82f62d38 | 610 | if (gv_IRValDiffsTwo[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] < -BEACON_SUSPECTED){ |
| re633 | 9:ef0907fda2f1 | 611 | testAfter = 2; |
| re633 | 9:ef0907fda2f1 | 612 | } |
| homero | 14:fc406dfff94f | 613 | } |
| homero | 14:fc406dfff94f | 614 | |
| re633 | 9:ef0907fda2f1 | 615 | //Firstly if the beacon is not detected by any of the sensors then change state back to search |
| re633 | 9:ef0907fda2f1 | 616 | if(testBefore == 0 && testDuring == 0 && testAfter == 0){ |
| homero | 13:c18d82f62d38 | 617 | changeState(States::SEARCHING_FWD); |
| homero | 14:fc406dfff94f | 618 | beacon_syncronised_flag = 1;//to exit while loop |
| re633 | 9:ef0907fda2f1 | 619 | |
| re633 | 9:ef0907fda2f1 | 620 | //If the tick before g_beaconOn is detecting the change caused by the flash change the value of g_beaconOn |
| re633 | 9:ef0907fda2f1 | 621 | } else if(testBefore == 1){ |
| re633 | 9:ef0907fda2f1 | 622 | g_beaconOn = g_beaconOn - 1; |
| re633 | 9:ef0907fda2f1 | 623 | |
| homero | 13:c18d82f62d38 | 624 | //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 |
| re633 | 9:ef0907fda2f1 | 625 | } else if(testBefore == 0 && testDuring == 1 && testAfter == 1){ |
| re633 | 9:ef0907fda2f1 | 626 | ticker_25ms.detach(); |
| homero | 13:c18d82f62d38 | 627 | tickChangeTimeout.attach_us(&atTimeout,10000); |
| homero | 13:c18d82f62d38 | 628 | wait(1);//Do not delete this wait |
| re633 | 9:ef0907fda2f1 | 629 | |
| re633 | 9:ef0907fda2f1 | 630 | //If successful the set flag |
| re633 | 9:ef0907fda2f1 | 631 | } else if (testBefore == 0 && testDuring == 1 && testAfter == 2){ |
| homero | 14:fc406dfff94f | 632 | beacon_syncronised_flag = 1; |
| re633 | 9:ef0907fda2f1 | 633 | |
| re633 | 9:ef0907fda2f1 | 634 | //Error handle. If this happens stop the piswarm |
| re633 | 9:ef0907fda2f1 | 635 | } else { |
| re633 | 9:ef0907fda2f1 | 636 | piswarm.set_oled_colour(255,255,255); |
| re633 | 9:ef0907fda2f1 | 637 | piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1); |
| re633 | 9:ef0907fda2f1 | 638 | piswarm.cls(); |
| re633 | 9:ef0907fda2f1 | 639 | piswarm.printf("%d %d %d",testBefore, testDuring,testAfter); |
| re633 | 9:ef0907fda2f1 | 640 | piswarm.stop(); |
| homero | 13:c18d82f62d38 | 641 | ticker_25ms.detach(); |
| homero | 13:c18d82f62d38 | 642 | tickChangeTimeout.attach_us(&atTimeout,10000); |
| homero | 13:c18d82f62d38 | 643 | wait_ms(500); |
| homero | 14:fc406dfff94f | 644 | beacon_syncronised_flag = 1; |
| homero | 13:c18d82f62d38 | 645 | changeState(States::SEARCHING_FWD); |
| re633 | 9:ef0907fda2f1 | 646 | } |
| re633 | 9:ef0907fda2f1 | 647 | } |
| re633 | 12:118f2b0ed8eb | 648 | } |
| homero | 14:fc406dfff94f | 649 | |
| re633 | 12:118f2b0ed8eb | 650 | //At target Beacon. |
| re633 | 12:118f2b0ed8eb | 651 | //Broadcast target beacon found and wait. |
| homero | 13:c18d82f62d38 | 652 | } else if (gv_state == States::AT_TARGET_BEACON){ |
| re633 | 12:118f2b0ed8eb | 653 | |
| re633 | 12:118f2b0ed8eb | 654 | piswarm.stop(); |
| re633 | 12:118f2b0ed8eb | 655 | piswarm.set_oled_colour(150,255,0); |
| re633 | 12:118f2b0ed8eb | 656 | piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1); |
| re633 | 12:118f2b0ed8eb | 657 | |
| homero | 13:c18d82f62d38 | 658 | //Detach tickers before broadcasting and waiting for reply |
| homero | 13:c18d82f62d38 | 659 | ticker_25ms.detach(); |
| homero | 13:c18d82f62d38 | 660 | ticker_ultrasonic50ms.detach(); |
| homero | 13:c18d82f62d38 | 661 | |
| homero | 13:c18d82f62d38 | 662 | uint8_t const num_to_broadcast = 10; |
| homero | 14:fc406dfff94f | 663 | |
| re633 | 12:118f2b0ed8eb | 664 | while(back_flag == 0){ |
| homero | 13:c18d82f62d38 | 665 | if(broadcasted_flag == 1 && broadcasted_count < num_to_broadcast){ |
| homero | 13:c18d82f62d38 | 666 | broadcastTimeout.attach_us(&atBroadcastTimeout,100000); |
| homero | 13:c18d82f62d38 | 667 | broadcasted_flag = 0; |
| homero | 13:c18d82f62d38 | 668 | } |
| re633 | 12:118f2b0ed8eb | 669 | } |
| homero | 13:c18d82f62d38 | 670 | |
| homero | 14:fc406dfff94f | 671 | ticker_25ms.attach_us(&readIRs,25000); |
| homero | 13:c18d82f62d38 | 672 | ticker_ultrasonic50ms.attach_us(&get_ultrasonic_readings,50000); |
| re633 | 12:118f2b0ed8eb | 673 | |
| re633 | 12:118f2b0ed8eb | 674 | //Return to beacon search state but now robot is lookling for the home beacon. |
| homero | 13:c18d82f62d38 | 675 | changeState(States::SEARCHING_FWD); |
| re633 | 12:118f2b0ed8eb | 676 | |
| re633 | 12:118f2b0ed8eb | 677 | //Back at home area. Stop and wait. |
| homero | 13:c18d82f62d38 | 678 | } else if (gv_state == States::AT_HOME_BEACON){ |
| re633 | 12:118f2b0ed8eb | 679 | piswarm.stop(); |
| re633 | 12:118f2b0ed8eb | 680 | piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1); |
| homero | 13:c18d82f62d38 | 681 | piswarm.play_tune( "T180L8O5EERERCL4EGR<GR",22 ); |
| re633 | 12:118f2b0ed8eb | 682 | while(1){ |
| re633 | 12:118f2b0ed8eb | 683 | piswarm.set_oled_colour(0,150,0); |
| homero | 13:c18d82f62d38 | 684 | wait_ms(200); |
| re633 | 12:118f2b0ed8eb | 685 | piswarm.set_oled_colour(150,0,0); |
| homero | 13:c18d82f62d38 | 686 | wait_ms(200); |
| re633 | 12:118f2b0ed8eb | 687 | piswarm.set_oled_colour(0,0,150); |
| homero | 13:c18d82f62d38 | 688 | wait_ms(200); |
| re633 | 12:118f2b0ed8eb | 689 | } |
| re633 | 9:ef0907fda2f1 | 690 | } |
| re633 | 9:ef0907fda2f1 | 691 | } |
| jah128 | 7:d03e54d9eb1c | 692 | } |
| jah128 | 7:d03e54d9eb1c | 693 | |
| jah128 | 2:e806b595f9ce | 694 | /*************************************************************************************************************************************** |
| jah128 | 2:e806b595f9ce | 695 | * |
| jah128 | 4:823174be9a6b | 696 | * Beyond this point, empty code blocks for optional functions is given |
| jah128 | 2:e806b595f9ce | 697 | * |
| jah128 | 4:823174be9a6b | 698 | * These may be left blank if not used, but should not be deleted |
| jah128 | 2:e806b595f9ce | 699 | * |
| jah128 | 2:e806b595f9ce | 700 | **************************************************************************************************************************************/ |
| jah128 | 2:e806b595f9ce | 701 | |
| jah128 | 1:37502eb3b70f | 702 | // Communications |
| jah128 | 1:37502eb3b70f | 703 | |
| jah128 | 1:37502eb3b70f | 704 | // 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 | 705 | // If the communication stack is not being used, all radio data is sent to processRawRFData() instead |
| jah128 | 1:37502eb3b70f | 706 | |
| jah128 | 1:37502eb3b70f | 707 | void handleUserRFCommand(char sender, char broadcast_message, char request_response, char id, char is_command, char function, char * data, char length){ |
| jah128 | 1:37502eb3b70f | 708 | // A 'user' RF Command has been received: write the code here to process it |
| jah128 | 1:37502eb3b70f | 709 | // sender = ID of the sender, range 0 to 31 |
| jah128 | 1:37502eb3b70f | 710 | // broadcast_message = 1 is message sent to all robots, 0 otherwise |
| jah128 | 1:37502eb3b70f | 711 | // request_response = 1 if a response is expected, 0 otherwise |
| jah128 | 1:37502eb3b70f | 712 | // id = Message ID, range 0 to 255 |
| jah128 | 1:37502eb3b70f | 713 | // 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 | 714 | // function = The function identifier. Range 0 to 15 |
| jah128 | 1:37502eb3b70f | 715 | // * data = Array containing extra data bytes |
| jah128 | 1:37502eb3b70f | 716 | // length = Length of extra data bytes held (range 0 to 57) |
| jah128 | 8:a789ef4fde52 | 717 | |
| jah128 | 7:d03e54d9eb1c | 718 | |
| jah128 | 4:823174be9a6b | 719 | //Do something... |
| re633 | 12:118f2b0ed8eb | 720 | piswarm.cls(); |
| re633 | 12:118f2b0ed8eb | 721 | piswarm.locate(0,0); |
| re633 | 12:118f2b0ed8eb | 722 | piswarm.printf("URF:%d",function); |
| re633 | 12:118f2b0ed8eb | 723 | if(length > 0) { |
| re633 | 12:118f2b0ed8eb | 724 | piswarm.locate(0,1); |
| re633 | 12:118f2b0ed8eb | 725 | piswarm.printf("%s",data); |
| re633 | 12:118f2b0ed8eb | 726 | } |
| re633 | 12:118f2b0ed8eb | 727 | |
| re633 | 12:118f2b0ed8eb | 728 | if(function == 1){ |
| re633 | 12:118f2b0ed8eb | 729 | start_flag = 1; |
| re633 | 12:118f2b0ed8eb | 730 | } |
| re633 | 12:118f2b0ed8eb | 731 | |
| re633 | 12:118f2b0ed8eb | 732 | if(function == 5){ |
| re633 | 12:118f2b0ed8eb | 733 | return_flag = 1; |
| re633 | 12:118f2b0ed8eb | 734 | } |
| re633 | 12:118f2b0ed8eb | 735 | if(function == 6){ |
| re633 | 12:118f2b0ed8eb | 736 | back_flag = 1; |
| re633 | 10:da62735d6df9 | 737 | } |
| jah128 | 1:37502eb3b70f | 738 | } |
| jah128 | 1:37502eb3b70f | 739 | |
| re633 | 12:118f2b0ed8eb | 740 | // This function is used to send the RF message: |
| re633 | 12:118f2b0ed8eb | 741 | void broadcast_user_rf_command(int function, char * message, int length) |
| re633 | 12:118f2b0ed8eb | 742 | { |
| re633 | 12:118f2b0ed8eb | 743 | //This function augments the communications stack |
| re633 | 12:118f2b0ed8eb | 744 | //It sends a 'user' RF command to all members (ie target_id = 0) |
| re633 | 12:118f2b0ed8eb | 745 | //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 | 746 | //It takes three inputs: |
| re633 | 12:118f2b0ed8eb | 747 | // * function (an integer from 0 to 15) |
| re633 | 12:118f2b0ed8eb | 748 | // * message (a char array) |
| re633 | 12:118f2b0ed8eb | 749 | // * length (length of message in bytes) |
| re633 | 12:118f2b0ed8eb | 750 | send_rf_message(0,48+(function % 16),message,length); |
| re633 | 12:118f2b0ed8eb | 751 | } |
| re633 | 12:118f2b0ed8eb | 752 | |
| jah128 | 1:37502eb3b70f | 753 | void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){ |
| jah128 | 1:37502eb3b70f | 754 | // A 'user' RF Response has been received: write the code here to process it |
| jah128 | 1:37502eb3b70f | 755 | // sender = ID of the sender, range 0 to 31 |
| jah128 | 1:37502eb3b70f | 756 | // broadcast_message = 1 is message sent to all robots, 0 otherwise |
| jah128 | 1:37502eb3b70f | 757 | // success = 1 if operation successful, 0 otherwise |
| jah128 | 1:37502eb3b70f | 758 | // id = Message ID, range 0 to 255 |
| jah128 | 1:37502eb3b70f | 759 | // 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 | 760 | // function = The function identifier. Range 0 to 15 |
| jah128 | 1:37502eb3b70f | 761 | // * data = Array containing extra data bytes |
| jah128 | 1:37502eb3b70f | 762 | // length = Length of extra data bytes held (range 0 to 57) |
| jah128 | 4:823174be9a6b | 763 | |
| jah128 | 4:823174be9a6b | 764 | //Do something... |
| jah128 | 1:37502eb3b70f | 765 | } |
| jah128 | 1:37502eb3b70f | 766 | |
| jah128 | 1:37502eb3b70f | 767 | void processRawRFData(char * rstring, char cCount){ |
| jah128 | 1:37502eb3b70f | 768 | // A raw RF packet has been received: write the code here to process it |
| jah128 | 1:37502eb3b70f | 769 | // rstring = The received packet |
| jah128 | 1:37502eb3b70f | 770 | // cCount = Packet length |
| jah128 | 4:823174be9a6b | 771 | |
| jah128 | 4:823174be9a6b | 772 | //Do something... |
| jah128 | 0:46cd1498a39a | 773 | } |
| jah128 | 0:46cd1498a39a | 774 | |
| jah128 | 1:37502eb3b70f | 775 | void switch_pressed() { |
| jah128 | 1:37502eb3b70f | 776 | //Switch(es) pressed {1 = Center 2 = Right 4 = Left 8 = Down 16 = Up} |
| jah128 | 1:37502eb3b70f | 777 | char switches = piswarm.get_switches(); |
| jah128 | 1:37502eb3b70f | 778 | |
| jah128 | 1:37502eb3b70f | 779 | //Do something... |
| homero | 13:c18d82f62d38 | 780 | } |