Diff: main.cpp

Revision:

10:da62735d6df9

Parent:

9:ef0907fda2f1

Child:

11:c5094a68283f

--- a/main.cpp	Wed Jul 22 15:49:52 2015 +0000
+++ b/main.cpp	Fri Jul 24 10:37:58 2015 +0000
@@ -24,6 +24,7 @@
 //Tickers
 Ticker ticker_25ms;
 Timer  timer;
+Timer timerLevy;
 
 //Global Variables
 uint8_t const IR_READ_PER_BEAC = 20; //The number of IR readings between beacon flashes
@@ -41,13 +42,14 @@
 int8_t g_beaconOn = 100;
 int8_t volatile tickBeaconSuspected = 100; //Is set to the tick value within the period between beacon flashes that the beacon flash is suspected to begin at
 int8_t tickBeaconPeriodCheck = 100; //Is used to temporarily store the value of tickBeaconSuspected
-
+uint16_t const TURN_BACK_TIME = (800/BASESPEED); //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 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
 uint8_t volatile flagBeaconIlluminated = 0; //Should be 1 when beacon is illuminated otherwise 0
-
+uint8_t flagSetNewLevyTime = 1; //Must start as 1
 //Ticker Function*************************************************************************************
 //This function is called by a ticker every 25ms
 //The function firstly stores the background IRS values.
@@ -146,15 +148,19 @@
     ticker_25ms.attach_us(&readIRs,25000);
     //starting point in state 11
     timer.start();
+    timerLevy.start();
     wait(1); //Wait a second to allow IR array to be filled
     
-    changeState(11);
+    //changeState(11);
     
     //Controller is a finite state machine
     while(1){
         
         //Waiting for signal to begin searching
         if(gv_state == 0){
+            if(flagSystemState == 1){
+                changeState(11);
+            }
             wait(1);
             
             //Change state here after recieving a radio command
@@ -197,20 +203,27 @@
                 //Secondly if obstacles are detected ahead then execute a random turn. 
                 if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
                     piswarm.stop();
+                    piswarm.cls(); 
+                    piswarm.printf("ob R");
+                    piswarm.play_tune("CC",1);
                     wait(0.1);
                     flagObstacle = 1;
                     changeState(12);
                 } else if(gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
                     piswarm.stop();
+                    piswarm.cls(); 
+                    piswarm.printf("ob L");
+                    piswarm.play_tune("CC",1);
                     wait(0.1);
                     flagObstacle = 2;
                     changeState(12);
-                
+                                             
+                //Otherwise continue moving forward until distance determined by levy algorithm is calculated. 
+                } else if(timerLevy.read_us() > levyTargetTimeus){
+                    flagSetNewLevyTime = 1;
+                    piswarm.play_tune("G",1);
+                    changeState(12);
                     
-            
-                //Otherwise continue moving forward until distance determined by levy algorithm is calculated. 
-                } else if(timer.read_us() > levyTargetTimeus){
-                    changeState(12);
                 } else if (flagStationary == 1){
                                
                     piswarm.forward(BASESPEED);
@@ -223,18 +236,25 @@
                 int16_t randomAngle;
                 //If sent here beacuse of obstacle find angle between -180 to -90 and 90 to 180
                 if(flagObstacle == 1){
-                    randomAngle = -90;
-                    flagObstacle = 0;
+                    randomAngle = rand()%90 - 135;
                     
                 } else if(flagObstacle == 2){
-                    randomAngle = 90;
-                    flagObstacle = 0;
-                //Otherwise if here due to levy walkturn to any random angle
+                    randomAngle = rand()%90 + 45;
+                    
+                //Otherwise if here due to levy walk: turn to any random angle
                 } else {
-                    randomAngle = rand()%360 - 180;
+                    randomAngle = rand()%360 - 180;                    
                 }
                 turnDegrees(randomAngle); //Make the turn
-                changeState(11);//Move back into state 11      
+                wait(0.1);
+                /* if(gv_IRDistances[0] < 70 || gv_IRDistances[1] < 70 || gv_IRDistances[6] < 70 || gv_IRDistances[7] < 70){
+                    //do nothing. Aim is that robot will keep turning the same way until clear of obstacle
+                    //could put a count in here to doemergency escape manouvre if necessary
+                     piswarm.play_tune("CC",1);
+                } else { */
+                flagObstacle = 0;
+                changeState(11);//Move back into state 11
+                      
             }           
             
             
@@ -412,6 +432,9 @@
     
        
     //Do something...
+    if(function == 1 && gv_state == 0) {
+        flagSystemState = 1;
+    }
 }    
 
 void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){