Diff: main.cpp

Revision:

12:118f2b0ed8eb

Parent:

11:c5094a68283f

Child:

13:c18d82f62d38

--- a/main.cpp	Wed Jul 29 13:58:57 2015 +0000
+++ b/main.cpp	Tue Aug 11 15:21:19 2015 +0000
@@ -14,9 +14,6 @@
 * copyright notice, this list of conditions and the following
 * disclaimer in the documentation and/or other materials provided
 * with the distribution.
-* * Neither the name of Willow Garage, Inc. nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
@@ -68,9 +65,10 @@
 int8_t g_beaconOn = 100;
 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
 int8_t tick_beacon_period_check = 100; //Is used to temporarily store the value of tick_beacon_suspected
-uint16_t const TURN_BACK_TIME = (800/BASE_SPEED); //Time between trying to continiue on path it wason before obstacle given by: (Distance in mm) / (robots speed)
 //Flags
-int8_t volatile flagSystemState = 0;
+int8_t volatile start_flag = 0;
+int8_t volatile back_flag = 0;
+int8_t volatile return_flag = 0;
 int8_t flagObstacle = 0;
 int8_t flagStationary = 1; //Used to determine if robot is currentl stationary or moving.
 uint8_t flagBeaconSyncronised = 0; //Set to one when the robot is synchronised with the beacon
@@ -184,16 +182,20 @@
         
         //Waiting for signal to begin searching
         if(gv_state == 0){
-            if(flagSystemState == 1){
+            if(start_flag == 1){
+                //Change state here after recieving a radio command
                 changeState(11);
             }
             wait(1);
             
-            //Change state here after recieving a radio command
+            
         //Searching state
         } else if (gv_state == 11 || gv_state == 12){
             
-            //Firstly determine if suspected beacon is actually the beacon.
+            //Do something here on receipt of 'function 5' if necessary.
+            //As currently the home beacon will immediately switch on that is not necessary.
+            
+            //Determine if suspected beacon is actually the beacon.
             //This is done by checking the period between flashes matches the beacon period
             if(tick_beacon_suspected != 100){
                 //piswarm.stop();
@@ -286,6 +288,10 @@
             
         //Beacon found state    
         } else if (gv_state == 2){
+            
+            //Do something here on receipt of 'function 5' if necessary.
+            //As currently the home beacon will immediately switch on that is not necessary.
+            
             int16_t maxValue[2] = {0,100};   //Value and sensor position  
             //wait(1);
             uint8_t loopCounter = 0;
@@ -368,8 +374,13 @@
                     //Should be at beacon
                     } else {
                         piswarm.stop();
-                        changeState(3);
                         
+                        //If either of these flags is one then the beacon should be the home beacon so change to state 4.
+                        if(return_flag == 1 || back_flag == 1){
+                            changeState(4);
+                        } else {
+                            changeState(3);
+                        }
                     }
                 }
             //Else need to syncronise with beacon
@@ -423,11 +434,49 @@
                         piswarm.cls();
                         piswarm.printf("%d %d %d",testBefore, testDuring,testAfter);
                         piswarm.stop();
-                        wait(5);
+                        wait(2);
                         changeState(11);
                     }
                 }
-            }              
+            }
+        //At target Beacon.
+        //Broadcast target beacon found and wait.             
+        } else if (gv_state == 3){
+            
+            
+            piswarm.stop();
+            piswarm.set_oled_colour(150,255,0);
+            piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
+            
+            //The value of function will change depending on the type of TAG used            
+            char function = 2;
+            char message[2] = {"0"};
+            broadcast_user_rf_command(function,message,1);
+            wait(0.5);
+            broadcast_user_rf_command(function,message,1);
+            wait(0.5);
+            broadcast_user_rf_command(function,message,1);
+            
+            while(back_flag == 0){
+                //Wait for the back flag to change
+                 wait(0.5);
+            }
+            
+            //Return to beacon search state but now robot is lookling for the home beacon.
+            changeState(11);
+            
+        //Back at home area. Stop and wait.
+        } else if (gv_state == 4){
+            piswarm.stop();
+            piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
+            while(1){
+                piswarm.set_oled_colour(0,150,0);
+                wait(0.2);
+                piswarm.set_oled_colour(150,0,0);
+                wait(0.2);
+                piswarm.set_oled_colour(0,0,150);
+                wait(0.2);
+            }
         }
     }  
 }
@@ -458,11 +507,39 @@
     
        
     //Do something...
-    if(function == 1 && gv_state == 0) {
-        flagSystemState = 1;
+    piswarm.cls();
+    piswarm.locate(0,0);
+    piswarm.printf("URF:%d",function);
+    if(length > 0) {
+        piswarm.locate(0,1);
+        piswarm.printf("%s",data);   
+    }
+
+    if(function == 1){
+        start_flag = 1;
+    }
+
+    if(function == 5){
+        return_flag = 1;
+    }
+    if(function == 6){
+        back_flag = 1;
     }
 }    
 
+// This function is used to send the RF message:
+void broadcast_user_rf_command(int function, char * message, int length)
+{
+    //This function augments the communications stack
+    //It sends a 'user' RF command to all members (ie target_id = 0)
+    //It sends a 'request', not a 'command', meaning it will still be handled if commands are disabled (RF_ALLOW_COMMANDS set to 0, recommended)
+    //It takes three inputs:
+    // * function (an integer from 0 to 15)
+    // * message  (a char array)
+    // * length   (length of message in bytes)
+    send_rf_message(0,48+(function % 16),message,length);
+}
+
 void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){
     // A 'user' RF Response has been received:  write the code here to process it
     // sender = ID of the sender, range 0 to 31