Diff: main.cpp

Revision:

9:ef0907fda2f1

Parent:

8:a789ef4fde52

Child:

10:da62735d6df9

--- a/main.cpp	Mon Dec 22 08:45:36 2014 +0000
+++ b/main.cpp	Wed Jul 22 15:49:52 2015 +0000
@@ -15,17 +15,375 @@
  ******************************************************************************************/
 
 #include "main.h"   // Certain parameters can be set by changing the defines in piswarm.h
+#include "PiSwarmControllerFunctions.h"
+
 
 PiSwarm piswarm;
 Serial pc (USBTX,USBRX);
 
+//Tickers
+Ticker ticker_25ms;
+Timer  timer;
+
+//Global Variables
+uint8_t const IR_READ_PER_BEAC = 20; //The number of IR readings between beacon flashes
+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
+int16_t volatile gv_IRValDiffs[IR_READ_PER_BEAC][8] = {0};
+uint8_t volatile beacon_detected[8] = {0}; 
+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)
+uint8_t const BEACON_SUSPECTED = 50; //Value by which consecutive IR sensor readings need to jump by for in order to cause beacon to be suspected.
+uint16_t const AT_BEACON_THRESHOLD = 3700;
+uint8_t volatile gv_state = 0; //This is the current state of the finite state machine
+uint32_t levyTargetTimeus = 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.
+uint8_t volatile gv_IRDistances[8]; //Using the custom distance function the active ir readings are converted to distances and stored here every 25ms.
+int16_t g_currentHeading = 0;
+float BASESPEED = 0.4;
+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
+
+//Flags
+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
+
+//Ticker Function*************************************************************************************
+//This function is called by a ticker every 25ms
+//The function firstly stores the background IRS values.
+//Secondly if beacon is off it uses illuminated IR sensors to estimate distances
+//Each second it will update when it believes beacon illumination time to be
+//It will work out which sensors spotted then beacon and will illuminate the Leds accordingly
+void readIRs(){
+    
+    //Fistly update the beaconVisable flag if possible
+    if(gv_counter25ms == mod8((g_beaconOn - 1), IR_READ_PER_BEAC)){
+        flagBeaconIlluminated = 1;
+    }
+    
+    if(gv_counter25ms == mod8((g_beaconOn + 2), IR_READ_PER_BEAC)){
+        flagBeaconIlluminated = 0;
+    }
+    //Firstly store background values
+    //Also make a note of which point in the second did the values change most.
+    //For which sensor specifically did the values change the most
+    //That sensor will be used to estimate the beacon start time if a threshold value is met
+    piswarm.store_background_raw_ir_values();
+    int16_t IRchange = 0;
+    uint8_t loopCounter = 0;
+    //In this loop the raw IR values are read.
+    //If the points where the IR values have increased by the greatest amount are noted as this indicates a beacon illumination
+    for(loopCounter = 0; loopCounter < 8; loopCounter++) {
+        gv_IRVals[gv_counter25ms][loopCounter] = piswarm.get_background_raw_ir_value(loopCounter);
+        IRchange = gv_IRVals[gv_counter25ms][loopCounter]-gv_IRVals[mod8((gv_counter25ms-1),IR_READ_PER_BEAC)][loopCounter];
+        
+        gv_IRValDiffs[gv_counter25ms][loopCounter] = IRchange;
+        //printf("change %d count %d\n",IRchange);
+        //If difference is greater than a threshold value then the beacon is suspected. This will be confirmed depending on the robots state of movement.
+        if (IRchange > BEACON_SUSPECTED){
+            tickBeaconSuspected = gv_counter25ms;
+            piswarm.cls();
+            piswarm.printf("%d",tickBeaconSuspected);
+        }        
+    }
+           
+    //Now store the illuminated values if the beacon is not illuminated-
+    piswarm.store_illuminated_raw_ir_values();
+     
+    //In this loop convert each raw active IR reading into a distance estimate   
+    for(loopCounter = 0; loopCounter < 8; loopCounter++) {
+        
+        //Specific sensor readings converted to distances
+        float temp = piswarm.get_illuminated_raw_ir_value(loopCounter);
+        if(temp>3500){
+           temp = 3500;
+        } else if (temp < 97){
+           temp = 97;
+        }
+        //#put this into a function 
+        //Switch case for robot 5
+        switch(loopCounter){
+            case 0:
+                temp = 662 * sqrt(1/(temp-148));
+                break;        
+            case 1:
+                temp = 662 * sqrt(1/(temp-144));
+                break; 
+            case 2:
+                temp = 662 * sqrt(1/(temp-120));
+                break;
+            case 3:
+                temp = 662 * sqrt(1/(temp-148));
+                break;
+            case 4:
+                temp = 662 * sqrt(1/(temp-120));
+                break;
+            case 5:
+                temp = 662 * sqrt(1/(temp-132));
+                break;
+            case 6:
+                temp = 662 * sqrt(1/(temp-152));
+                break;
+            case 7:
+                temp = 662 * sqrt(1/(temp-212));
+                break;
+        }
+              
+        if (temp > 130){
+            temp = 130;
+        }
+        gv_IRDistances[loopCounter] = temp;            
+
+    }     
+    //reset counter after 1 second (beacon period)
+    gv_counter25ms = mod8(gv_counter25ms + 1,IR_READ_PER_BEAC);
+}
+
+//*******************************************************************************************************
 //This is where the program code goes.  
 int main() {
-    init();
+    init();  
+    ticker_25ms.attach_us(&readIRs,25000);
+    //starting point in state 11
+    timer.start();
+    wait(1); //Wait a second to allow IR array to be filled
+    
+    changeState(11);
     
-    while(1) {
-       
-    }
+    //Controller is a finite state machine
+    while(1){
+        
+        //Waiting for signal to begin searching
+        if(gv_state == 0){
+            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.
+            //This is done by checking the period between flashes matches the beacon period
+            if(tickBeaconSuspected != 100){
+                //piswarm.stop();
+                //When the beacon flag is first raised store its value and reset it
+                if(tickBeaconPeriodCheck == 100){
+                    tickBeaconPeriodCheck = tickBeaconSuspected;
+                    tickBeaconSuspected = 100;
+                //Check the timing of the latest jump with the last one to see if period matches the Beacon.
+                } else {
+                    piswarm.locate(0,1);
+                    piswarm.printf("%d %d",tickBeaconPeriodCheck,tickBeaconSuspected);
+                    //printf("%d %d *********************************",tickBeaconPeriodCheck,tickBeaconSuspected);
+                    //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.
+                    int8_t test = (tickBeaconPeriodCheck - tickBeaconSuspected);
+                    
+                    test = test * test;
+                    
+                    //if test is low then identify the beacon as the cause of the flags
+                    if(test < 2){
+                        //Beacon found change to state 2
+                        g_beaconOn = tickBeaconPeriodCheck; //update the global variable that stores when beacon flashes occur
+                        
+                        wait(2);
+                        changeState(2);
+                    } else {
+                        //Reset the flag to try again
+                        tickBeaconPeriodCheck = 100;
+                    }
+                }
+            }
+            
+            if(gv_state == 11){
+                //Secondly if obstacles are detected ahead then execute a random turn. 
+                if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
+                    piswarm.stop();
+                    wait(0.1);
+                    flagObstacle = 1;
+                    changeState(12);
+                } else if(gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
+                    piswarm.stop();
+                    wait(0.1);
+                    flagObstacle = 2;
+                    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);
+                    flagStationary = 0;
+                }                
+                
+            } else if(gv_state == 12){
+                piswarm.stop();//Stop the robot.
+                flagStationary = 1; //update this flag
+                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;
+                    
+                } else if(flagObstacle == 2){
+                    randomAngle = 90;
+                    flagObstacle = 0;
+                //Otherwise if here due to levy walkturn to any random angle
+                } else {
+                    randomAngle = rand()%360 - 180;
+                }
+                turnDegrees(randomAngle); //Make the turn
+                changeState(11);//Move back into state 11      
+            }           
+            
+            
+        //Beacon found state    
+        } else if (gv_state == 2){
+            int16_t maxValue[2] = {0,100};   //Value and sensor position  
+            //wait(1);
+            uint8_t loopCounter = 0;
+
+            //If beacon visible
+            if(flagBeaconSyncronised == 1){
+                
+                //Firstly check beacon is still visible
+                flagBeaconSyncronised = 0;
+                //Update array concerning which IRs can see the beacon
+                for(loopCounter = 0; loopCounter<8; loopCounter++) {
+                                       
+                    //Find which sensor has the highest reading         
+                    if( gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED) {
+                        if(gv_IRVals[g_beaconOn][loopCounter] > maxValue[0]){
+                            maxValue[0] = gv_IRVals[g_beaconOn][loopCounter];
+                            maxValue[1] = loopCounter;
+                        }
+                        flagBeaconSyncronised = 1; //This will remain as one so long as at least on sensor can see beacon
+                    }
+                }
+                
+                //Only do this if beacon still visible
+                if(flagBeaconSyncronised == 1){
+                    
+                    //If the adjacent two sensors are above the threshold too then they can also be marked as illuminated
+                    for(loopCounter = 0; loopCounter<8; loopCounter++){
+                        
+                        //reset all beacon detected values
+                        beacon_detected[loopCounter] = 0;
+
+                        if(abs(maxValue[1] - loopCounter)< 3 || abs(maxValue[1] + 8 - loopCounter)< 3 || abs(maxValue[1] - 8 - loopCounter)< 3) {
+                            if(gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED){
+                                beacon_detected[loopCounter] = 1;
+                            }
+                        }
+                    }
+                
+            
+                    //Update the piswarm LEDS so the ones that can see the beacon are on.
+                    piswarm.set_oleds(beacon_detected[0]||beacon_detected[1],
+                        beacon_detected[1]||beacon_detected[2],
+                        beacon_detected[2],
+                        beacon_detected[3],
+                        0,
+                        beacon_detected[4],
+                        beacon_detected[5],
+                        beacon_detected[5]||beacon_detected[6],
+                        beacon_detected[6]||beacon_detected[7],
+                        beacon_detected[7]||beacon_detected[0]);
+                
+                    //If the max IR value is below a threshold then move toward beacon. Else change state
+                    if(maxValue[0] < AT_BEACON_THRESHOLD){
+                        
+                        //Calculate the heading of Pi-Swarm Relative to beacon
+                        //printf("%d ",g_currentHeading);
+                        calculateNewHeading();
+                        printf("%d ",g_currentHeading);
+                        if(g_currentHeading > 5 ||g_currentHeading < -5){
+                            turnDegrees(-g_currentHeading);
+                        }
+                        printf("%d\n",g_currentHeading);    
+                        //}
+                        
+                        
+                        
+                        //If the beacon is not currently on but obstacle detected then do obstacle avoidance
+                        
+                        if(flagBeaconIlluminated == 0){
+                            if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
+                                turnDegrees(90);
+                            } else if (gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
+                                turnDegrees(-90);
+                            }
+                        }
+                        
+                        
+                        piswarm.forward(0.2);
+                        wait(1);
+                    //Should be at beacon
+                    } else {
+                        piswarm.stop();
+                        changeState(3);
+                        
+                    }
+                }
+            //Else need to syncronise with beacon
+            } else {
+                
+                while(flagBeaconSyncronised == 0){  
+                    //Sychronise the ticker with the beacon
+                    
+                    uint8_t testBefore = 0;
+                    uint8_t testDuring = 0;
+                    uint8_t testAfter = 0;
+                    for(loopCounter = 0; loopCounter < 8; loopCounter++){
+                        if (gv_IRValDiffs[mod8((g_beaconOn - 1),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){
+                            testBefore = 1;
+                        }
+                        if (gv_IRValDiffs[g_beaconOn][loopCounter] > BEACON_SUSPECTED){
+                            testDuring = 1;
+                        }
+                        if (gv_IRValDiffs[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] > BEACON_SUSPECTED){
+                            testAfter = 1;
+                        }
+                        if (gv_IRValDiffs[mod8((g_beaconOn + 2),IR_READ_PER_BEAC)][loopCounter] < -BEACON_SUSPECTED){
+                            testAfter = 2;
+                        }
+                    } 
+                    //Firstly if the beacon is not detected by any of the sensors then change state back to search
+                    if(testBefore == 0 && testDuring == 0 && testAfter == 0){
+                        changeState(11);
+                        flagBeaconSyncronised = 1;//to exit while loop
+                    
+                    //If the tick before g_beaconOn is detecting the change caused by the flash change the value of g_beaconOn 
+                    } else if(testBefore == 1){
+                        g_beaconOn = g_beaconOn - 1;
+                    
+                    //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 15ms
+                    } else if(testBefore == 0 && testDuring == 1 && testAfter == 1){
+                        ticker_25ms.detach();
+                        timer.reset();
+                        while(timer.read_ms() < 100){};
+                        ticker_25ms.attach_us(&readIRs,25000);
+                        wait(1);
+                    
+                    //If successful the set flag
+                    } else if (testBefore == 0 && testDuring == 1 && testAfter == 2){
+                        flagBeaconSyncronised = 1;
+                    
+                    //Error handle. If this happens stop the piswarm
+                    } else {
+                        piswarm.set_oled_colour(255,255,255);
+                        piswarm.set_oleds(1,1,1,1,1,1,1,1,1,1);
+                        piswarm.cls();
+                        piswarm.printf("%d %d %d",testBefore, testDuring,testAfter);
+                        piswarm.stop();
+                        wait(5);
+                        changeState(11);
+                    }
+                }
+            }              
+        }
+    }  
 }
 
 /***************************************************************************************************************************************