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@7:d03e54d9eb1c, 2014-06-11 (annotated)

Committer:
jah128
Date:
Wed Jun 11 14:16:30 2014 +0000
Revision:
7:d03e54d9eb1c
Parent:
5:70a83245e681
Child:
8:a789ef4fde52
test;

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
jah128 0:46cd1498a39a 18
jah128 7:d03e54d9eb1c 19 #define rearSensors 90
jah128 7:d03e54d9eb1c 20 #define frontSensors 90
jah128 7:d03e54d9eb1c 21 #define wallsSensors 55
jah128 7:d03e54d9eb1c 22 #define cohesion 50
jah128 7:d03e54d9eb1c 23 #define separation 75
jah128 7:d03e54d9eb1c 24
jah128 7:d03e54d9eb1c 25 uint8_t loop_counter = 0;
jah128 7:d03e54d9eb1c 26 float sensor_data[8] = {0};
jah128 7:d03e54d9eb1c 27 volatile uint8_t proximity_signal[4] = {0};
jah128 7:d03e54d9eb1c 28 float speed = 0;
jah128 7:d03e54d9eb1c 29 float speed_incrementor = 0;
jah128 7:d03e54d9eb1c 30 float delay = 0;
jah128 7:d03e54d9eb1c 31
jah128 7:d03e54d9eb1c 32 DigitalOut irFront(p12);
jah128 7:d03e54d9eb1c 33 DigitalOut irBack(p13);
jah128 7:d03e54d9eb1c 34 Timer t;
jah128 7:d03e54d9eb1c 35 Timer speed_timer;
jah128 7:d03e54d9eb1c 36
jah128 0:46cd1498a39a 37
jah128 1:37502eb3b70f 38 PiSwarm piswarm;
jah128 1:37502eb3b70f 39 Serial pc (USBTX,USBRX);
jah128 0:46cd1498a39a 40
jah128 1:37502eb3b70f 41 //This is where the program code goes. In the simple demo, the outer LEDs are blinked.
jah128 1:37502eb3b70f 42 int main() {
jah128 1:37502eb3b70f 43 init();
jah128 7:d03e54d9eb1c 44 t.start();
jah128 7:d03e54d9eb1c 45 speed_timer.start();
jah128 7:d03e54d9eb1c 46 speed = 0.15;
jah128 1:37502eb3b70f 47 while(1) {
jah128 7:d03e54d9eb1c 48
jah128 7:d03e54d9eb1c 49 speed_incrementor = 0.08 + speed / 10;
jah128 7:d03e54d9eb1c 50 delay = 0.12 - (speed / 10) * 4;
jah128 7:d03e54d9eb1c 51 get_sensors_values();
jah128 7:d03e54d9eb1c 52 if( speed_timer.read_ms() > 1000 ){
jah128 7:d03e54d9eb1c 53 if( speed > 0.05 ) speed -= 0.005;
jah128 7:d03e54d9eb1c 54 for( loop_counter = 0; loop_counter < 3; loop_counter++ ){
jah128 7:d03e54d9eb1c 55 if( proximity_signal[loop_counter] == 1 && speed < 0.2 ) speed += 0.005;
jah128 7:d03e54d9eb1c 56 }
jah128 7:d03e54d9eb1c 57 speed_timer.reset();
jah128 7:d03e54d9eb1c 58 //piswarm.cls();
jah128 7:d03e54d9eb1c 59 //piswarm.printf("%0.2f %d%d%d\n\r", speed, proximity_signal[0], proximity_signal[1], proximity_signal[2]);
jah128 7:d03e54d9eb1c 60 }
jah128 7:d03e54d9eb1c 61
jah128 7:d03e54d9eb1c 62 //Forward and backward condition
jah128 7:d03e54d9eb1c 63 while( ( sensor_data[0] + sensor_data[7] ) / 2 > ( separation + 15 ) )
jah128 7:d03e54d9eb1c 64 {
jah128 7:d03e54d9eb1c 65 attraction();
jah128 7:d03e54d9eb1c 66 piswarm.forward( speed );
jah128 7:d03e54d9eb1c 67 piswarm.set_oleds( 1, 0, 0, 0, 0, 0, 0, 0, 1, 1 );
jah128 7:d03e54d9eb1c 68 get_sensors_values();
jah128 7:d03e54d9eb1c 69 wait( delay );
jah128 7:d03e54d9eb1c 70 }
jah128 7:d03e54d9eb1c 71 while( ( sensor_data[0] + sensor_data[7] ) / 2 < ( separation - 15 ) && ( sensor_data[3] + sensor_data[4] ) / 2 > ( separation - 15 ) )
jah128 7:d03e54d9eb1c 72 {
jah128 7:d03e54d9eb1c 73 piswarm.backward( speed_incrementor );
jah128 7:d03e54d9eb1c 74 piswarm.set_oleds( 0, 0, 0, 1, 1, 1, 0, 0, 0, 0 );
jah128 7:d03e54d9eb1c 75 get_sensors_values();
jah128 7:d03e54d9eb1c 76 wait( delay );
jah128 1:37502eb3b70f 77 }
jah128 7:d03e54d9eb1c 78 //Front-left separation
jah128 7:d03e54d9eb1c 79 while( sensor_data[6] < separation || sensor_data[5] < separation )
jah128 7:d03e54d9eb1c 80 {
jah128 7:d03e54d9eb1c 81 t.stop();
jah128 7:d03e54d9eb1c 82 t.reset();
jah128 7:d03e54d9eb1c 83 piswarm.left( speed_incrementor );
jah128 7:d03e54d9eb1c 84 piswarm.set_oleds( 0, 0, 0, 0, 0, 0, 1, 1, 0, 0 );
jah128 7:d03e54d9eb1c 85 get_sensors_values();
jah128 7:d03e54d9eb1c 86 wait( delay );
jah128 7:d03e54d9eb1c 87 t.start();
jah128 1:37502eb3b70f 88 }
jah128 7:d03e54d9eb1c 89
jah128 7:d03e54d9eb1c 90 //Front-right separation
jah128 7:d03e54d9eb1c 91 while( sensor_data[1] < separation || sensor_data[2] < separation)
jah128 7:d03e54d9eb1c 92 {
jah128 7:d03e54d9eb1c 93 t.stop();
jah128 7:d03e54d9eb1c 94 t.reset();
jah128 7:d03e54d9eb1c 95 piswarm.right( speed_incrementor );
jah128 7:d03e54d9eb1c 96 piswarm.set_oleds( 0, 1, 1, 0, 0, 0, 0, 0, 0, 0 );
jah128 7:d03e54d9eb1c 97 get_sensors_values();
jah128 7:d03e54d9eb1c 98 wait( delay );
jah128 7:d03e54d9eb1c 99 t.start();
jah128 7:d03e54d9eb1c 100 }
jah128 7:d03e54d9eb1c 101 piswarm.stop();
jah128 7:d03e54d9eb1c 102 attraction();
jah128 7:d03e54d9eb1c 103 }
jah128 7:d03e54d9eb1c 104 }
jah128 7:d03e54d9eb1c 105
jah128 7:d03e54d9eb1c 106 void get_sensors_values()
jah128 7:d03e54d9eb1c 107 {
jah128 7:d03e54d9eb1c 108 for(loop_counter = 0; loop_counter < 8; loop_counter++ ){
jah128 7:d03e54d9eb1c 109 sensor_data[loop_counter] = 0.0;
jah128 7:d03e54d9eb1c 110 sensor_data[loop_counter] = piswarm.read_reflected_ir_distance(loop_counter);
jah128 0:46cd1498a39a 111 }
jah128 0:46cd1498a39a 112 }
jah128 0:46cd1498a39a 113
jah128 7:d03e54d9eb1c 114 void attraction(void){
jah128 7:d03e54d9eb1c 115 if(t.read_ms() > 2500){//limit for going without seeing any robot, otherwise check where they are
jah128 7:d03e54d9eb1c 116 t.stop();
jah128 7:d03e54d9eb1c 117 t.reset();
jah128 7:d03e54d9eb1c 118 irFront = 0;
jah128 7:d03e54d9eb1c 119 irBack = 0;
jah128 7:d03e54d9eb1c 120 int maxIrValue = 0;
jah128 7:d03e54d9eb1c 121 int maxIrLoc = 0;
jah128 7:d03e54d9eb1c 122 unsigned short irValue = 0;
jah128 7:d03e54d9eb1c 123 for(int i=0;i<8;i++){
jah128 7:d03e54d9eb1c 124 piswarm.read_adc_value(i);
jah128 7:d03e54d9eb1c 125 }
jah128 7:d03e54d9eb1c 126 for(int i=0;i<8;i++){
jah128 7:d03e54d9eb1c 127 irValue = piswarm.read_adc_value(i);
jah128 7:d03e54d9eb1c 128 if(irValue > maxIrValue){
jah128 7:d03e54d9eb1c 129 maxIrValue = irValue;
jah128 7:d03e54d9eb1c 130 maxIrLoc = i;
jah128 7:d03e54d9eb1c 131 }
jah128 7:d03e54d9eb1c 132 }
jah128 7:d03e54d9eb1c 133 t.start();
jah128 7:d03e54d9eb1c 134 t.reset();
jah128 7:d03e54d9eb1c 135 switch(maxIrLoc){
jah128 7:d03e54d9eb1c 136 case 0:
jah128 7:d03e54d9eb1c 137 piswarm.right(0.36);
jah128 7:d03e54d9eb1c 138 while(t.read_ms() < 30){//15 degrees
jah128 7:d03e54d9eb1c 139 }
jah128 7:d03e54d9eb1c 140 piswarm.stop();
jah128 7:d03e54d9eb1c 141 t.stop();
jah128 7:d03e54d9eb1c 142 break;
jah128 7:d03e54d9eb1c 143
jah128 7:d03e54d9eb1c 144 case 1:
jah128 7:d03e54d9eb1c 145 piswarm.right(0.36);
jah128 7:d03e54d9eb1c 146 while(t.read_ms() < 90){//45 degrees
jah128 7:d03e54d9eb1c 147 }
jah128 7:d03e54d9eb1c 148 piswarm.stop();
jah128 7:d03e54d9eb1c 149 t.stop();
jah128 7:d03e54d9eb1c 150 break;
jah128 7:d03e54d9eb1c 151
jah128 7:d03e54d9eb1c 152 case 2:
jah128 7:d03e54d9eb1c 153 piswarm.right(0.36);
jah128 7:d03e54d9eb1c 154 while(t.read_ms() < 180){//90 degrees
jah128 7:d03e54d9eb1c 155 }
jah128 7:d03e54d9eb1c 156 piswarm.stop();
jah128 7:d03e54d9eb1c 157 t.stop();
jah128 7:d03e54d9eb1c 158 break;
jah128 7:d03e54d9eb1c 159
jah128 7:d03e54d9eb1c 160 case 3:
jah128 7:d03e54d9eb1c 161 piswarm.right(0.36);
jah128 7:d03e54d9eb1c 162 while(t.read_ms() < 288){//144 degrees
jah128 7:d03e54d9eb1c 163 }
jah128 7:d03e54d9eb1c 164 piswarm.stop();
jah128 7:d03e54d9eb1c 165 t.stop();
jah128 7:d03e54d9eb1c 166 break;
jah128 7:d03e54d9eb1c 167
jah128 7:d03e54d9eb1c 168 case 4:
jah128 7:d03e54d9eb1c 169 piswarm.left(0.36);
jah128 7:d03e54d9eb1c 170 while(t.read_ms() < 288){//216 degrees
jah128 7:d03e54d9eb1c 171 }
jah128 7:d03e54d9eb1c 172 piswarm.stop();
jah128 7:d03e54d9eb1c 173 t.stop();
jah128 7:d03e54d9eb1c 174 break;
jah128 7:d03e54d9eb1c 175
jah128 7:d03e54d9eb1c 176 case 5:
jah128 7:d03e54d9eb1c 177 piswarm.left(0.36);
jah128 7:d03e54d9eb1c 178 while(t.read_ms() < 180){//270 degrees
jah128 7:d03e54d9eb1c 179 }
jah128 7:d03e54d9eb1c 180 piswarm.stop();
jah128 7:d03e54d9eb1c 181 t.stop();
jah128 7:d03e54d9eb1c 182 break;
jah128 7:d03e54d9eb1c 183
jah128 7:d03e54d9eb1c 184 case 6:
jah128 7:d03e54d9eb1c 185 piswarm.left(0.36);
jah128 7:d03e54d9eb1c 186 while(t.read_ms() < 90){//315 degrees
jah128 7:d03e54d9eb1c 187 }
jah128 7:d03e54d9eb1c 188 piswarm.stop();
jah128 7:d03e54d9eb1c 189 t.stop();
jah128 7:d03e54d9eb1c 190 break;
jah128 7:d03e54d9eb1c 191
jah128 7:d03e54d9eb1c 192 case 7:
jah128 7:d03e54d9eb1c 193 piswarm.left(0.36);
jah128 7:d03e54d9eb1c 194 while(t.read_ms() < 30){//345 degrees
jah128 7:d03e54d9eb1c 195 }
jah128 7:d03e54d9eb1c 196 piswarm.stop();
jah128 7:d03e54d9eb1c 197 t.stop();
jah128 7:d03e54d9eb1c 198 break;
jah128 7:d03e54d9eb1c 199 }
jah128 7:d03e54d9eb1c 200 t.start();
jah128 7:d03e54d9eb1c 201 irFront = 1;
jah128 7:d03e54d9eb1c 202 irBack = 1;
jah128 7:d03e54d9eb1c 203 }
jah128 7:d03e54d9eb1c 204
jah128 7:d03e54d9eb1c 205 }
jah128 2:e806b595f9ce 206 /***************************************************************************************************************************************
jah128 2:e806b595f9ce 207 *
jah128 4:823174be9a6b 208 * Beyond this point, empty code blocks for optional functions is given
jah128 2:e806b595f9ce 209 *
jah128 4:823174be9a6b 210 * These may be left blank if not used, but should not be deleted
jah128 2:e806b595f9ce 211 *
jah128 2:e806b595f9ce 212 **************************************************************************************************************************************/
jah128 2:e806b595f9ce 213
jah128 1:37502eb3b70f 214 // Communications
jah128 1:37502eb3b70f 215
jah128 1:37502eb3b70f 216 // 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 217 // If the communication stack is not being used, all radio data is sent to processRawRFData() instead
jah128 1:37502eb3b70f 218
jah128 1:37502eb3b70f 219 void handleUserRFCommand(char sender, char broadcast_message, char request_response, char id, char is_command, char function, char * data, char length){
jah128 1:37502eb3b70f 220 // A 'user' RF Command has been received: write the code here to process it
jah128 1:37502eb3b70f 221 // sender = ID of the sender, range 0 to 31
jah128 1:37502eb3b70f 222 // broadcast_message = 1 is message sent to all robots, 0 otherwise
jah128 1:37502eb3b70f 223 // request_response = 1 if a response is expected, 0 otherwise
jah128 1:37502eb3b70f 224 // id = Message ID, range 0 to 255
jah128 1:37502eb3b70f 225 // 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 226 // function = The function identifier. Range 0 to 15
jah128 1:37502eb3b70f 227 // * data = Array containing extra data bytes
jah128 1:37502eb3b70f 228 // length = Length of extra data bytes held (range 0 to 57)
jah128 7:d03e54d9eb1c 229 proximity_signal[0] = data[0];
jah128 7:d03e54d9eb1c 230 proximity_signal[1] = data[1];
jah128 7:d03e54d9eb1c 231 proximity_signal[2] = data[2];
jah128 7:d03e54d9eb1c 232 piswarm.cls();
jah128 7:d03e54d9eb1c 233 piswarm.printf("%0.2f %d%d%d", speed, proximity_signal[0], proximity_signal[1], proximity_signal[2]);
jah128 7:d03e54d9eb1c 234
jah128 4:823174be9a6b 235 //Do something...
jah128 1:37502eb3b70f 236 }
jah128 1:37502eb3b70f 237
jah128 1:37502eb3b70f 238 void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){
jah128 1:37502eb3b70f 239 // A 'user' RF Response has been received: write the code here to process it
jah128 1:37502eb3b70f 240 // sender = ID of the sender, range 0 to 31
jah128 1:37502eb3b70f 241 // broadcast_message = 1 is message sent to all robots, 0 otherwise
jah128 1:37502eb3b70f 242 // success = 1 if operation successful, 0 otherwise
jah128 1:37502eb3b70f 243 // id = Message ID, range 0 to 255
jah128 1:37502eb3b70f 244 // 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 245 // function = The function identifier. Range 0 to 15
jah128 1:37502eb3b70f 246 // * data = Array containing extra data bytes
jah128 1:37502eb3b70f 247 // length = Length of extra data bytes held (range 0 to 57)
jah128 4:823174be9a6b 248
jah128 4:823174be9a6b 249 //Do something...
jah128 1:37502eb3b70f 250 }
jah128 1:37502eb3b70f 251
jah128 1:37502eb3b70f 252 void processRawRFData(char * rstring, char cCount){
jah128 1:37502eb3b70f 253 // A raw RF packet has been received: write the code here to process it
jah128 1:37502eb3b70f 254 // rstring = The received packet
jah128 1:37502eb3b70f 255 // cCount = Packet length
jah128 4:823174be9a6b 256
jah128 4:823174be9a6b 257 //Do something...
jah128 0:46cd1498a39a 258 }
jah128 0:46cd1498a39a 259
jah128 1:37502eb3b70f 260 void switch_pressed() {
jah128 1:37502eb3b70f 261 //Switch(es) pressed {1 = Center 2 = Right 4 = Left 8 = Down 16 = Up}
jah128 1:37502eb3b70f 262 char switches = piswarm.get_switches();
jah128 1:37502eb3b70f 263
jah128 1:37502eb3b70f 264 //Do something...
jah128 0:46cd1498a39a 265 }