Edgar Buchanan / Mbed 2 deprecated Aggregation-Flocking_2

Aggregation-Flocking_2

Dependencies:   Pi_Swarm_Library_v06_alpha mbed

Fork of Pi_Swarm_Blank by piswarm

main.cpp@5:66e1a4c974dd, 2014-06-11 (annotated)

Committer:
edgarbuchanan
Date:
Wed Jun 11 13:50:09 2014 +0000
Revision:
5:66e1a4c974dd
Parent:
4:823174be9a6b
Child:
6:a13d06616ae1
Flock code

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
edgarbuchanan 5:66e1a4c974dd 19 #define rearSensors 90
edgarbuchanan 5:66e1a4c974dd 20 #define frontSensors 90
edgarbuchanan 5:66e1a4c974dd 21 #define wallsSensors 55
edgarbuchanan 5:66e1a4c974dd 22 #define cohesion 50
edgarbuchanan 5:66e1a4c974dd 23 #define separation 75
edgarbuchanan 5:66e1a4c974dd 24
edgarbuchanan 5:66e1a4c974dd 25 uint8_t loop_counter = 0;
edgarbuchanan 5:66e1a4c974dd 26 float sensor_data[8] = {0};
edgarbuchanan 5:66e1a4c974dd 27 volatile uint8_t proximity_signal[4] = {0};
edgarbuchanan 5:66e1a4c974dd 28 float speed = 0;
edgarbuchanan 5:66e1a4c974dd 29 float speed_incrementor = 0;
edgarbuchanan 5:66e1a4c974dd 30 float delay = 0;
edgarbuchanan 5:66e1a4c974dd 31
edgarbuchanan 5:66e1a4c974dd 32 DigitalOut irFront(p12);
edgarbuchanan 5:66e1a4c974dd 33 DigitalOut irBack(p13);
edgarbuchanan 5:66e1a4c974dd 34 Timer t;
edgarbuchanan 5:66e1a4c974dd 35 Timer speed_timer;
edgarbuchanan 5:66e1a4c974dd 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();
edgarbuchanan 5:66e1a4c974dd 44 t.start();
edgarbuchanan 5:66e1a4c974dd 45 speed_timer.start();
edgarbuchanan 5:66e1a4c974dd 46 speed = 0.15;
jah128 1:37502eb3b70f 47 while(1) {
edgarbuchanan 5:66e1a4c974dd 48
edgarbuchanan 5:66e1a4c974dd 49 speed_incrementor = 0.08 + speed / 10;
edgarbuchanan 5:66e1a4c974dd 50 delay = 0.12 - (speed / 10) * 4;
edgarbuchanan 5:66e1a4c974dd 51 get_sensors_values();
edgarbuchanan 5:66e1a4c974dd 52 if( speed_timer.read_ms() > 1000 ){
edgarbuchanan 5:66e1a4c974dd 53 if( speed > 0.05 ) speed -= 0.005;
edgarbuchanan 5:66e1a4c974dd 54 for( loop_counter = 0; loop_counter < 3; loop_counter++ ){
edgarbuchanan 5:66e1a4c974dd 55 if( proximity_signal[loop_counter] == 1 && speed < 0.2 ) speed += 0.005;
edgarbuchanan 5:66e1a4c974dd 56 }
edgarbuchanan 5:66e1a4c974dd 57 speed_timer.reset();
edgarbuchanan 5:66e1a4c974dd 58 //piswarm.cls();
edgarbuchanan 5:66e1a4c974dd 59 //piswarm.printf("%0.2f %d%d%d\n\r", speed, proximity_signal[0], proximity_signal[1], proximity_signal[2]);
edgarbuchanan 5:66e1a4c974dd 60 }
edgarbuchanan 5:66e1a4c974dd 61
edgarbuchanan 5:66e1a4c974dd 62 //Forward and backward condition
edgarbuchanan 5:66e1a4c974dd 63 while( ( sensor_data[0] + sensor_data[7] ) / 2 > ( separation + 15 ) )
edgarbuchanan 5:66e1a4c974dd 64 {
edgarbuchanan 5:66e1a4c974dd 65 attraction();
edgarbuchanan 5:66e1a4c974dd 66 piswarm.forward( speed );
edgarbuchanan 5:66e1a4c974dd 67 piswarm.set_oleds( 1, 0, 0, 0, 0, 0, 0, 0, 1, 1 );
edgarbuchanan 5:66e1a4c974dd 68 get_sensors_values();
edgarbuchanan 5:66e1a4c974dd 69 wait( delay );
edgarbuchanan 5:66e1a4c974dd 70 }
edgarbuchanan 5:66e1a4c974dd 71 while( ( sensor_data[0] + sensor_data[7] ) / 2 < ( separation - 15 ) && ( sensor_data[3] + sensor_data[4] ) / 2 > ( separation - 15 ) )
edgarbuchanan 5:66e1a4c974dd 72 {
edgarbuchanan 5:66e1a4c974dd 73 piswarm.backward( speed_incrementor );
edgarbuchanan 5:66e1a4c974dd 74 piswarm.set_oleds( 0, 0, 0, 1, 1, 1, 0, 0, 0, 0 );
edgarbuchanan 5:66e1a4c974dd 75 get_sensors_values();
edgarbuchanan 5:66e1a4c974dd 76 wait( delay );
jah128 1:37502eb3b70f 77 }
edgarbuchanan 5:66e1a4c974dd 78 //Front-left separation
edgarbuchanan 5:66e1a4c974dd 79 while( sensor_data[6] < separation || sensor_data[5] < separation )
edgarbuchanan 5:66e1a4c974dd 80 {
edgarbuchanan 5:66e1a4c974dd 81 t.stop();
edgarbuchanan 5:66e1a4c974dd 82 t.reset();
edgarbuchanan 5:66e1a4c974dd 83 piswarm.left( speed_incrementor );
edgarbuchanan 5:66e1a4c974dd 84 piswarm.set_oleds( 0, 0, 0, 0, 0, 0, 1, 1, 0, 0 );
edgarbuchanan 5:66e1a4c974dd 85 get_sensors_values();
edgarbuchanan 5:66e1a4c974dd 86 wait( delay );
edgarbuchanan 5:66e1a4c974dd 87 t.start();
jah128 1:37502eb3b70f 88 }
edgarbuchanan 5:66e1a4c974dd 89
edgarbuchanan 5:66e1a4c974dd 90 //Front-right separation
edgarbuchanan 5:66e1a4c974dd 91 while( sensor_data[1] < separation || sensor_data[2] < separation)
edgarbuchanan 5:66e1a4c974dd 92 {
edgarbuchanan 5:66e1a4c974dd 93 t.stop();
edgarbuchanan 5:66e1a4c974dd 94 t.reset();
edgarbuchanan 5:66e1a4c974dd 95 piswarm.right( speed_incrementor );
edgarbuchanan 5:66e1a4c974dd 96 piswarm.set_oleds( 0, 1, 1, 0, 0, 0, 0, 0, 0, 0 );
edgarbuchanan 5:66e1a4c974dd 97 get_sensors_values();
edgarbuchanan 5:66e1a4c974dd 98 wait( delay );
edgarbuchanan 5:66e1a4c974dd 99 t.start();
edgarbuchanan 5:66e1a4c974dd 100 }
edgarbuchanan 5:66e1a4c974dd 101 piswarm.stop();
edgarbuchanan 5:66e1a4c974dd 102 attraction();
edgarbuchanan 5:66e1a4c974dd 103 }
edgarbuchanan 5:66e1a4c974dd 104 }
edgarbuchanan 5:66e1a4c974dd 105
edgarbuchanan 5:66e1a4c974dd 106 void get_sensors_values()
edgarbuchanan 5:66e1a4c974dd 107 {
edgarbuchanan 5:66e1a4c974dd 108 for(loop_counter = 0; loop_counter < 8; loop_counter++ ){
edgarbuchanan 5:66e1a4c974dd 109 sensor_data[loop_counter] = 0.0;
edgarbuchanan 5:66e1a4c974dd 110 sensor_data[loop_counter] = piswarm.read_reflected_ir_distance(loop_counter);
jah128 0:46cd1498a39a 111 }
jah128 0:46cd1498a39a 112 }
jah128 0:46cd1498a39a 113
edgarbuchanan 5:66e1a4c974dd 114 void attraction(void){
edgarbuchanan 5:66e1a4c974dd 115 if(t.read_ms() > 2500){//limit for going without seeing any robot, otherwise check where they are
edgarbuchanan 5:66e1a4c974dd 116 t.stop();
edgarbuchanan 5:66e1a4c974dd 117 t.reset();
edgarbuchanan 5:66e1a4c974dd 118 irFront = 0;
edgarbuchanan 5:66e1a4c974dd 119 irBack = 0;
edgarbuchanan 5:66e1a4c974dd 120 int maxIrValue = 0;
edgarbuchanan 5:66e1a4c974dd 121 int maxIrLoc = 0;
edgarbuchanan 5:66e1a4c974dd 122 unsigned short irValue = 0;
edgarbuchanan 5:66e1a4c974dd 123 for(int i=0;i<8;i++){
edgarbuchanan 5:66e1a4c974dd 124 piswarm.read_adc_value(i);
edgarbuchanan 5:66e1a4c974dd 125 }
edgarbuchanan 5:66e1a4c974dd 126 for(int i=0;i<8;i++){
edgarbuchanan 5:66e1a4c974dd 127 irValue = piswarm.read_adc_value(i);
edgarbuchanan 5:66e1a4c974dd 128 if(irValue > maxIrValue){
edgarbuchanan 5:66e1a4c974dd 129 maxIrValue = irValue;
edgarbuchanan 5:66e1a4c974dd 130 maxIrLoc = i;
edgarbuchanan 5:66e1a4c974dd 131 }
edgarbuchanan 5:66e1a4c974dd 132 }
edgarbuchanan 5:66e1a4c974dd 133 t.start();
edgarbuchanan 5:66e1a4c974dd 134 t.reset();
edgarbuchanan 5:66e1a4c974dd 135 switch(maxIrLoc){
edgarbuchanan 5:66e1a4c974dd 136 case 0:
edgarbuchanan 5:66e1a4c974dd 137 piswarm.right(0.36);
edgarbuchanan 5:66e1a4c974dd 138 while(t.read_ms() < 30){//15 degrees
edgarbuchanan 5:66e1a4c974dd 139 }
edgarbuchanan 5:66e1a4c974dd 140 piswarm.stop();
edgarbuchanan 5:66e1a4c974dd 141 t.stop();
edgarbuchanan 5:66e1a4c974dd 142 break;
edgarbuchanan 5:66e1a4c974dd 143
edgarbuchanan 5:66e1a4c974dd 144 case 1:
edgarbuchanan 5:66e1a4c974dd 145 piswarm.right(0.36);
edgarbuchanan 5:66e1a4c974dd 146 while(t.read_ms() < 90){//45 degrees
edgarbuchanan 5:66e1a4c974dd 147 }
edgarbuchanan 5:66e1a4c974dd 148 piswarm.stop();
edgarbuchanan 5:66e1a4c974dd 149 t.stop();
edgarbuchanan 5:66e1a4c974dd 150 break;
edgarbuchanan 5:66e1a4c974dd 151
edgarbuchanan 5:66e1a4c974dd 152 case 2:
edgarbuchanan 5:66e1a4c974dd 153 piswarm.right(0.36);
edgarbuchanan 5:66e1a4c974dd 154 while(t.read_ms() < 180){//90 degrees
edgarbuchanan 5:66e1a4c974dd 155 }
edgarbuchanan 5:66e1a4c974dd 156 piswarm.stop();
edgarbuchanan 5:66e1a4c974dd 157 t.stop();
edgarbuchanan 5:66e1a4c974dd 158 break;
edgarbuchanan 5:66e1a4c974dd 159
edgarbuchanan 5:66e1a4c974dd 160 case 3:
edgarbuchanan 5:66e1a4c974dd 161 piswarm.right(0.36);
edgarbuchanan 5:66e1a4c974dd 162 while(t.read_ms() < 288){//144 degrees
edgarbuchanan 5:66e1a4c974dd 163 }
edgarbuchanan 5:66e1a4c974dd 164 piswarm.stop();
edgarbuchanan 5:66e1a4c974dd 165 t.stop();
edgarbuchanan 5:66e1a4c974dd 166 break;
edgarbuchanan 5:66e1a4c974dd 167
edgarbuchanan 5:66e1a4c974dd 168 case 4:
edgarbuchanan 5:66e1a4c974dd 169 piswarm.left(0.36);
edgarbuchanan 5:66e1a4c974dd 170 while(t.read_ms() < 288){//216 degrees
edgarbuchanan 5:66e1a4c974dd 171 }
edgarbuchanan 5:66e1a4c974dd 172 piswarm.stop();
edgarbuchanan 5:66e1a4c974dd 173 t.stop();
edgarbuchanan 5:66e1a4c974dd 174 break;
edgarbuchanan 5:66e1a4c974dd 175
edgarbuchanan 5:66e1a4c974dd 176 case 5:
edgarbuchanan 5:66e1a4c974dd 177 piswarm.left(0.36);
edgarbuchanan 5:66e1a4c974dd 178 while(t.read_ms() < 180){//270 degrees
edgarbuchanan 5:66e1a4c974dd 179 }
edgarbuchanan 5:66e1a4c974dd 180 piswarm.stop();
edgarbuchanan 5:66e1a4c974dd 181 t.stop();
edgarbuchanan 5:66e1a4c974dd 182 break;
edgarbuchanan 5:66e1a4c974dd 183
edgarbuchanan 5:66e1a4c974dd 184 case 6:
edgarbuchanan 5:66e1a4c974dd 185 piswarm.left(0.36);
edgarbuchanan 5:66e1a4c974dd 186 while(t.read_ms() < 90){//315 degrees
edgarbuchanan 5:66e1a4c974dd 187 }
edgarbuchanan 5:66e1a4c974dd 188 piswarm.stop();
edgarbuchanan 5:66e1a4c974dd 189 t.stop();
edgarbuchanan 5:66e1a4c974dd 190 break;
edgarbuchanan 5:66e1a4c974dd 191
edgarbuchanan 5:66e1a4c974dd 192 case 7:
edgarbuchanan 5:66e1a4c974dd 193 piswarm.left(0.36);
edgarbuchanan 5:66e1a4c974dd 194 while(t.read_ms() < 30){//345 degrees
edgarbuchanan 5:66e1a4c974dd 195 }
edgarbuchanan 5:66e1a4c974dd 196 piswarm.stop();
edgarbuchanan 5:66e1a4c974dd 197 t.stop();
edgarbuchanan 5:66e1a4c974dd 198 break;
edgarbuchanan 5:66e1a4c974dd 199 }
edgarbuchanan 5:66e1a4c974dd 200 t.start();
edgarbuchanan 5:66e1a4c974dd 201 irFront = 1;
edgarbuchanan 5:66e1a4c974dd 202 irBack = 1;
edgarbuchanan 5:66e1a4c974dd 203 }
edgarbuchanan 5:66e1a4c974dd 204
edgarbuchanan 5:66e1a4c974dd 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)
edgarbuchanan 5:66e1a4c974dd 229 proximity_signal[0] = data[0];
edgarbuchanan 5:66e1a4c974dd 230 proximity_signal[1] = data[1];
edgarbuchanan 5:66e1a4c974dd 231 proximity_signal[2] = data[2];
edgarbuchanan 5:66e1a4c974dd 232 piswarm.cls();
edgarbuchanan 5:66e1a4c974dd 233 piswarm.printf("%0.2f %d%d%d", speed, proximity_signal[0], proximity_signal[1], proximity_signal[2]);
edgarbuchanan 5:66e1a4c974dd 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 }