Diff: main.cpp

Revision:

14:fc406dfff94f

Parent:

13:c18d82f62d38

Child:

15:f7a8989a3cd3

--- a/main.cpp	Sun Aug 23 14:26:12 2015 +0000
+++ b/main.cpp	Sun Aug 23 16:27:14 2015 +0000
@@ -45,7 +45,7 @@
 * @communication added
 */
 #include "main.h"   // Certain parameters can be set by changing the defines in piswarm.h
-#include "PiSwarmControllerFunctions.h"
+#include "pi_swarm_functions.h"
 #include "hcsr04.h"
 
 PiSwarm piswarm;
@@ -53,58 +53,61 @@
 HCSR04 usensor(p30,p14);
 
 //Tickers
-Ticker ticker_25ms;
-Ticker ticker_ultrasonic50ms;
+Ticker ticker_25ms; //Ticker used to periodically obtain new IR readings.
+Ticker ticker_ultrasonic50ms; //Ticker used to periodically obtain a new ultrasonic reading.
 
 //Timeouts
-Timeout tickChangeTimeout;
-Timeout broadcastTimeout;
+Timeout tickChangeTimeout; //Timeout used to adjust IR read ticker phase to enable to beacon syncronisation.
+Timeout broadcastTimeout; //Timeout used for broadcasting messages multiple times with a set interval.
 
 //Timers
-Timer  timer;
-Timer timerLevy;
+Timer turn_timer; //Timer used to turn a user specified number of degrees.
+Timer levy_timer; //Timer used to control time between levy walk steps.
 
 //Global Variables
+
+//Individucal Pi-Swarm variables: speed and motor trim etc
+float BASE_SPEED = 0.6; //The input to the motor functions when the robot is moving forward and does not detect obstacles.
+float r_mot_scaler = 1; //This variable is allows particular pi swarms to be adjusted to travel in a straight line.
+float l_mot_scaler = 1; //This variable is allows particular pi swarms to be adjusted to travel in a straight line.
+uint16_t at_beacon_threshold = 3800; //A raw IR reading greater than this value indicates the robot is at a beacon.
+int16_t g_currentHeading = 0; //The current heading of the Pi-swarm relative to the IR beacon.
+uint8_t robot_id; //The id unique to each Pi-Swarm robot
+
+//Finite state machine information
+uint8_t volatile gv_state = States::READY_TO_START; //This is the current state of the finite state machine
+int8_t g_obstacle_type = 0; //The Pi-Swarm will perform different obstacle avoidance behaviours depending on which sensors detect obstacles. This variable is used to note which sensor detected the obstacle.
+
+//IR Readings + beacon syncronisation 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};
-int16_t volatile gv_IRValDiffsTwo[IR_READ_PER_BEAC][8] = {0};
-
-uint8_t volatile beacon_detected[8] = {0}; 
+int16_t volatile gv_IRValDiffs[IR_READ_PER_BEAC][8] = {0};//The difference between consective IR readings.
+int16_t volatile gv_IRValDiffsTwo[IR_READ_PER_BEAC][8] = {0};//The difference between IR readings seperated by two ticks
+uint8_t volatile beacon_detected[8] = {0}; //An IR sensor is judged to have a detected the beacon in a valid way the corresponding array variable will be set to 1, else 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)
+int8_t g_beaconOn = 100; //The first tick at which the beacon is illuminated
 uint8_t const BEACON_SUSPECTED = 20; //Value by which consecutive IR sensor readings need to jump by for in order to cause beacon to be suspected.
-uint16_t volatile AT_BEACON_THRESHOLD = 3800;
-float volatile distance_ultrasonic_sensor = 1000;
-
-float r_mot_scaler = 1;
-float l_mot_scaler = 1;
-
-uint32_t levy_target_time_us = 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.
-
-//homero: ultrasonic reading
-
-uint8_t volatile gv_state = States::READY_TO_START; //This is the current state of the finite state machine
-int16_t g_currentHeading = 0;
-float BASE_SPEED = 0.6;
-int8_t g_beaconOn = 100;
+uint8_t volatile gv_IRDistances[8]; //Using the custom distance function the active IR readings are converted to distances and stored here every 25ms.
 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
-uint8_t g_ultrasound_count = 0;
-uint8_t broadcasted_count = 0;
-uint8_t go_back_count = 0;
-//Flags
-int8_t volatile start_flag = 1;
-int8_t volatile back_flag = 0;
-int8_t volatile return_flag = 0;
-//int8_t volatile aligned_target_beacon = 0;  //homero
-int8_t flagObstacle = 0;
-uint8_t flagStationary = 1; //Used to determine if robot is currently 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
-uint8_t broadcasted_flag = 1;
-uint8_t robot_id;
+uint8_t beacon_syncronised_flag = 0; //Set to one when the robot is synchronised with the beacon
+uint8_t volatile beacon_illuminated_flag = 0; //Should be 1 when beacon is illuminated otherwise 0
+
+//Ultrasonic Readings
+float volatile distance_ultrasonic_sensor = 1000; //Here is stored the latest reading from the ultrasonic sensor in mm from 1 -800. 1000 is an error value.
+
+//Levy walk variables
+uint32_t levy_target_time_us = 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 g_back_count = 0; //After a set number of Levy walk turns the robot will move backward. This variable monitors how many Levy turns since the last backward move.
+uint8_t set_new_levy_time_flag = 1; //Must start as 1. If fwd move state is entered following a levy turn the flag will be 1 indicating a new time should be set.
+
+
+//Communication 
+uint8_t broadcasted_flag = 1; //Set to one when a message has been broadcast. 0 when about to broadcast a new message
+uint8_t broadcasted_count = 0; //The number of messages that have been broadcasted in one batch
+int8_t volatile start_flag = 1; //Set to 1 on receiving a command by RF. Indicates robot can start searching for target beacon.
+int8_t volatile back_flag = 0;  //Set to 1 on receiving a command by RF. Indicates robot can start searching for home beacon, unless it is at the target beacon.
+int8_t volatile return_flag = 0; //Set to 1 on receiving a command by RF. Indicates all robots now start or continue searching for home beacon.
 
 //Ticker Function*************************************************************************************
 //This function is called by a ticker every 25ms
@@ -116,11 +119,11 @@
     
     //Fistly update the beaconVisable flag if possible
     if(gv_counter25ms == mod8((g_beaconOn - 1), IR_READ_PER_BEAC)){
-        flagBeaconIlluminated = 1;
+        beacon_illuminated_flag = 1;
     }
     
     if(gv_counter25ms == mod8((g_beaconOn + 2), IR_READ_PER_BEAC)){
-        flagBeaconIlluminated = 0;
+        beacon_illuminated_flag = 0;
     }
     //Firstly store background values
     //Also make a note of which point in the second did the values change most.
@@ -308,35 +311,35 @@
                 ticker_25ms.attach_us(&readIRs,25000);
                 
                 if(robot_id == 1){
-                    AT_BEACON_THRESHOLD = 3850;
+                    at_beacon_threshold = 3850;
                 }
                     
                 else if (robot_id == 9){
-                    AT_BEACON_THRESHOLD = 3950;
+                    at_beacon_threshold = 3950;
                 }
                 
                 else if (robot_id == 12){
-                    AT_BEACON_THRESHOLD = 3980;
+                    at_beacon_threshold = 3980;
                     r_mot_scaler = 1.02;
                     l_mot_scaler = 0.98;
                 }
                 
                 else{
-                    AT_BEACON_THRESHOLD = 3900;
+                    at_beacon_threshold = 3900;
                 }
                 
                 usensor.start();  // get first reading of the ultrasonic sensor required before ticker 
                 wait_ms(50);
                 ticker_ultrasonic50ms.attach_us(&get_ultrasonic_readings,50000);
                 
-                timer.start();
-                timerLevy.start();
+                turn_timer.start();
+                levy_timer.start();
                 //pc.printf("why\n");
                 changeState(States::SEARCHING_FWD);
             }
     
         //Searching state
-        } else if (gv_state == States::SEARCHING_FWD || gv_state == States::SEARCHING_TURNING || gv_state == States::SEARCHING_REDUCED_SPEED){
+        } else if (gv_state == States::SEARCHING_FWD || gv_state == States::SEARCHING_TURNING){
             
             //Do something here on receipt of 'function 5' if necessary.
             //As currently the home beacon will immediately switch on that is not necessary.
@@ -373,30 +376,14 @@
             }
             
             if(gv_state == States::SEARCHING_FWD){
-                //Secondly if obstacles are detected ahead then execute a random turn. 
-                
-                //pc.printf("Start\n");
-                 
-                //Homero: This is the obstacle avoidance part need to also turn if the ultrasound reading is high
-                //Avoid obstacle to the right
-                //if(distance_ultrasonic_sensor < 500 && distance_ultrasonic_sensor >= 100)
-//                {
-//                    g_ultrasound_count++;              
-//                    
-//                } else {
-//                    g_ultrasound_count = 0;
-//                }
-                         
-                //if (distance_ultrasonic_sensor < 500 && distance_ultrasonic_sensor >= 100 && g_ultrasound_count > 3){
-//                    changeState(States::SEARCHING_REDUCED_SPEED);
+               
                     
                 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;
+                    g_obstacle_type = 1;
                     changeState(States::SEARCHING_TURNING);
                 
                 //Avoid obstacle to the left
@@ -405,33 +392,31 @@
                     piswarm.cls(); 
                     piswarm.printf("ob L");
                     piswarm.play_tune("CC",1);
-                    //wait(0.1);
-                    flagObstacle = 2;
+                    g_obstacle_type = 2;
                     changeState(States::SEARCHING_TURNING);  
                 
-                } else if(distance_ultrasonic_sensor < 50){
+                } else if(distance_ultrasonic_sensor < 70){
                     piswarm.stop();
                     piswarm.cls(); 
                     piswarm.printf("ob F");
-                    //piswarm.printf("F: %f",distance_ultrasonic_sensor);
                     piswarm.play_tune("DD",1);
                     //wait(0.1);
-                    flagObstacle = 3;
+                    g_obstacle_type = 3;
                     changeState(States::SEARCHING_TURNING);
                     
                     
                 } else if(distance_ultrasonic_sensor <= 80){
-                   flagObstacle = 3;
+                   g_obstacle_type = 3;
                    changeState(States::SEARCHING_TURNING);
                    
-                } else if(timerLevy.read_us() > levy_target_time_us){
-                    go_back_count++;
-                    flagSetNewLevyTime = 1;
+                } else if(levy_timer.read_us() > levy_target_time_us){
+                    g_back_count++;
+                    set_new_levy_time_flag = 1;
                     piswarm.play_tune("G",1);
                     piswarm.set_oled_colour(255,0,0);
                     changeState(States::SEARCHING_TURNING);
-                }else if(go_back_count >=10){
-                    go_back_count = 0;
+                }else if(g_back_count >=8){
+                    g_back_count = 0;
                     piswarm.stop();
                     wait_ms(200);
                     piswarm.left_motor(-0.4*l_mot_scaler);
@@ -456,8 +441,6 @@
                 } else {
                     piswarm.right_motor(BASE_SPEED*r_mot_scaler);
                     piswarm.left_motor(BASE_SPEED*l_mot_scaler);           
-                    //wait_ms(200);
-                    //flagStationary = 0;
                 }
                 //wait to get new ultrasound reading
                 //wait_ms(50);  
@@ -467,73 +450,23 @@
                 piswarm.stop();//Stop the robot.
                 int16_t randomAngle;
                 //If sent here beacuse of obstacle find angle between -180 to -90 and 90 to 180
-                if(flagObstacle == 1){
+                if(g_obstacle_type == 1){
                     randomAngle = rand()%90 - 135;
                     
-                } else if(flagObstacle == 2){
+                } else if(g_obstacle_type == 2){
                     randomAngle = rand()%90 + 45;
                     
-                } else if(flagObstacle == 3){
-                randomAngle = rand()%90 + 45;
+                } else if(g_obstacle_type == 3){
+                    randomAngle = rand()%90 + 45;
                     
                 //Otherwise if here due to levy walk: turn to any random angle
                 } else {
                     randomAngle = rand()%360 - 180;                    
                 }
                 turnDegrees(randomAngle); //Make the turn
-                //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;
+                g_obstacle_type = 0;
                 changeState(States::SEARCHING_FWD);//Move back into state 11
                     
-            } else if (gv_state == States::SEARCHING_REDUCED_SPEED){
-                
-               
-                
-                if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
-                    piswarm.stop();
-                    piswarm.cls(); 
-                    piswarm.printf("ob R");
-                    piswarm.play_tune("CC",1);
-                    flagObstacle = 1;
-                    changeState(States::SEARCHING_TURNING);
-                
-                //Avoid obstacle to the left
-                } else if(gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
-                    piswarm.stop();
-                    piswarm.cls(); 
-                    piswarm.printf("ob L");
-                    piswarm.play_tune("CC",1);
-                    flagObstacle = 2;
-                    changeState(States::SEARCHING_TURNING);  
-                      
-                } else if(timerLevy.read_us() > levy_target_time_us){
-                    flagSetNewLevyTime = 1;
-                    piswarm.play_tune("G",1);
-                    changeState(States::SEARCHING_TURNING);
-                
-                } else if(distance_ultrasonic_sensor > 800 || distance_ultrasonic_sensor == 0){
-                    changeState(States::SEARCHING_FWD);
-                    
-                } else if(distance_ultrasonic_sensor <= 50){
-                   flagObstacle = 3;
-                   changeState(States::SEARCHING_TURNING);
-                }else {
-                   //decrease speed when PiSwarm is getting close to an obstacle
-                   float velocity = 0.2;
-                   if(distance_ultrasonic_sensor > 100){
-                       velocity = (distance_ultrasonic_sensor*0.00043) + 0.157; 
-                   }  
-                   piswarm.left_motor(velocity);
-                   piswarm.right_motor(velocity);
-                   //wait_ms(0.2);
-                }
-                    
-             }           
         //Beacon found state    
         } else if (gv_state == States::MOVING_TO_BEACON){
             
@@ -541,23 +474,21 @@
             //As currently the home beacon will immediately switch on that is not necessary.
             
             static int16_t valid_distances[8] = {0};
-           
             
             int16_t maxValue[2] = {0,100};   //Value and sensor position  
-            //wait(1);
             int8_t loopCounter = 0;
             
-            if(flagBeaconIlluminated == 0){
+            if(beacon_illuminated_flag == 0){
                 for(loopCounter = 0; loopCounter < 8; loopCounter++){
                     valid_distances[loopCounter] = gv_IRDistances[loopCounter];
                 }
             }
 
             //If beacon visible
-            if(flagBeaconSyncronised == 1){
+            if(beacon_syncronised_flag == 1){
                 
                 //Firstly check beacon is still visible
-                flagBeaconSyncronised = 0;
+                beacon_syncronised_flag = 0;
                 //Update array concerning which IRs can see the beacon
                 for(loopCounter = 0; loopCounter<8; loopCounter++) {
                                        
@@ -567,14 +498,14 @@
                             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
+                                beacon_syncronised_flag = 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(beacon_syncronised_flag == 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++){
@@ -603,19 +534,18 @@
                         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){
+                    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);
                         }
                         
                         //If the beacon is not currently on but obstacle detected then do obstacle avoidance
                         int16_t randomAngle;
-                        if(flagBeaconIlluminated == 0){
+                        if(beacon_illuminated_flag == 0){
                             if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
                                 randomAngle = rand()%90 - 135;
                                 piswarm.stop();
@@ -623,8 +553,6 @@
                                 piswarm.backward(0.3);
                                 wait_ms(200);
                                 turnDegrees(randomAngle);
-                                //piswarm.forward(0.2);
-//                                wait_ms(500);
                             } else if (gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
                                 randomAngle = rand()%90 + 45;
                                 piswarm.stop();
@@ -632,8 +560,6 @@
                                 piswarm.backward(0.3);
                                 wait_ms(200);
                                 turnDegrees(randomAngle);
-                               // piswarm.forward(0.2);
-//                                wait_ms(500);
                             } else if ( distance_ultrasonic_sensor < 100){
                                 randomAngle = rand()%60 - 30;
                                 piswarm.stop();
@@ -641,18 +567,15 @@
                                 piswarm.backward(0.3);
                                 wait_ms(200);
                                 turnDegrees(randomAngle);
-                                //piswarm.forward(0.2);
-//                                wait_ms(500);
                             }
                         }
-                    piswarm.right_motor(0.3*r_mot_scaler*r_mot_scaler);                           
-                    piswarm.left_motor(0.3*l_mot_scaler*l_mot_scaler); 
-                    wait_ms(500);   
+                        piswarm.right_motor(0.3*r_mot_scaler*r_mot_scaler);                           
+                        piswarm.left_motor(0.3*l_mot_scaler*l_mot_scaler); 
+                        wait_ms(500);   
                     //Should be at beacon
                     } else {
                         piswarm.stop();
                         calculateNewHeading();
-                        //printf("%d ",g_currentHeading);
                         if(g_currentHeading > 5 || g_currentHeading < -5){
                             turnDegrees(-g_currentHeading);
                         }
@@ -668,9 +591,9 @@
             //Else need to syncronise with beacon
             } else {
                 
-                while(flagBeaconSyncronised == 0){  
+                while(beacon_syncronised_flag == 0){
+                      
                     //Sychronise the ticker with the beacon
-                    
                     uint8_t testBefore = 0;
                     uint8_t testDuring = 0;
                     uint8_t testAfter = 0;
@@ -687,11 +610,12 @@
                         if (gv_IRValDiffsTwo[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(States::SEARCHING_FWD);
-                        flagBeaconSyncronised = 1;//to exit while loop
+                        beacon_syncronised_flag = 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){
@@ -700,13 +624,12 @@
                     //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 10ms
                     } else if(testBefore == 0 && testDuring == 1 && testAfter == 1){
                         ticker_25ms.detach();
-                        //This was 100000? I think it should be 10000
                         tickChangeTimeout.attach_us(&atTimeout,10000);
                         wait(1);//Do not delete this wait
                     
                     //If successful the set flag
                     } else if (testBefore == 0 && testDuring == 1 && testAfter == 2){
-                        flagBeaconSyncronised = 1;
+                        beacon_syncronised_flag = 1;
                     
                     //Error handle. If this happens stop the piswarm
                     } else {
@@ -718,11 +641,12 @@
                         ticker_25ms.detach();
                         tickChangeTimeout.attach_us(&atTimeout,10000);
                         wait_ms(500);
-                        flagBeaconSyncronised = 1;
+                        beacon_syncronised_flag = 1;
                         changeState(States::SEARCHING_FWD);
                     }
                 }
             }
+            
         //At target Beacon.
         //Broadcast target beacon found and wait.             
         } else if (gv_state == States::AT_TARGET_BEACON){
@@ -736,34 +660,15 @@
             ticker_ultrasonic50ms.detach();
             
             uint8_t const num_to_broadcast = 10;
-            
-/*            while(aligned_target_beacon ==0){
-                if(gv_IRVals[gv_counter25ms][7] < AT_BEACON_THRESHOLD && gv_IRVals[gv_counter25ms][0] < AT_BEACON_THRESHOLD  ){
-                    calculateNewHeading();
-                    wait(0.025);
-                    turnDegrees(-g_currentHeading);
-                    
-                }
-                else if (gv_IRVals[gv_counter25ms][7] > AT_BEACON_THRESHOLD && gv_IRVals[gv_counter25ms][0] > AT_BEACON_THRESHOLD) {
-                    aligned_target_beacon = 1;
-                }
-                
-                else{
-                    piswarm.printf("stuck");
-                }
-            }*/
-   
-            
+             
             while(back_flag == 0){
                 if(broadcasted_flag == 1 && broadcasted_count < num_to_broadcast){
                     broadcastTimeout.attach_us(&atBroadcastTimeout,100000);
                     broadcasted_flag = 0;
                 }
-                //Wait for the back flag to change
-                 //wait_us(1000);
             }
 
-                    ticker_25ms.attach_us(&readIRs,25000);
+            ticker_25ms.attach_us(&readIRs,25000);
             ticker_ultrasonic50ms.attach_us(&get_ultrasonic_readings,50000);
             
             //Return to beacon search state but now robot is lookling for the home beacon.