Diff: main.cpp

Revision:

11:c5094a68283f

Parent:

10:da62735d6df9

Child:

12:118f2b0ed8eb

--- a/main.cpp	Fri Jul 24 10:37:58 2015 +0000
+++ b/main.cpp	Wed Jul 29 13:58:57 2015 +0000
@@ -1,19 +1,45 @@
-/*******************************************************************************************
- *
- * University of York Robot Lab Pi Swarm Robot Library
- *
- * "Blank" Program
- *
- * Use this file as the template to produce custom controllers
- *
- * (C) Dr James Hilder, Dept. Electronics & Computer Science, University of York
- * 
- * Version 0.6  February 2014
- *
- * Designed for use with the Pi Swarm Board (enhanced MBED sensor board) v1.2
- *
- ******************************************************************************************/
-
+/*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2015, University of York Robotics Laboratory (YRL).
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* 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
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*/
+/**
+* @file main.cpp
+* @brief The Ticker fucntion for gaining sensor data and the main function for the PRGP Pi-Swarm Controllers.
+* @details In this file the main function for the Pi-Swarm Controller is defined.
+* @version 1.0
+* @author Robert
+* @author Evans
+* @date 24/07/15
+*/
 #include "main.h"   // Certain parameters can be set by changing the defines in piswarm.h
 #include "PiSwarmControllerFunctions.h"
 
@@ -35,14 +61,14 @@
 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.
+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.
 int16_t g_currentHeading = 0;
-float BASESPEED = 0.4;
+float BASE_SPEED = 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
-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)
+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 flagObstacle = 0;
@@ -83,9 +109,9 @@
         //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;
+            tick_beacon_suspected = gv_counter25ms;
             piswarm.cls();
-            piswarm.printf("%d",tickBeaconSuspected);
+            piswarm.printf("%d",tick_beacon_suspected);
         }        
     }
            
@@ -169,32 +195,32 @@
             
             //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){
+            if(tick_beacon_suspected != 100){
                 //piswarm.stop();
                 //When the beacon flag is first raised store its value and reset it
-                if(tickBeaconPeriodCheck == 100){
-                    tickBeaconPeriodCheck = tickBeaconSuspected;
-                    tickBeaconSuspected = 100;
+                if(tick_beacon_period_check == 100){
+                    tick_beacon_period_check = tick_beacon_suspected;
+                    tick_beacon_suspected = 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);
+                    piswarm.printf("%d %d",tick_beacon_period_check,tick_beacon_suspected);
+                    //printf("%d %d *********************************",tick_beacon_period_check,tick_beacon_suspected);
                     //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);
+                    int8_t test = (tick_beacon_period_check - tick_beacon_suspected);
                     
                     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
+                        g_beaconOn = tick_beacon_period_check; //update the global variable that stores when beacon flashes occur
                         
                         wait(2);
                         changeState(2);
                     } else {
                         //Reset the flag to try again
-                        tickBeaconPeriodCheck = 100;
+                        tick_beacon_period_check = 100;
                     }
                 }
             }
@@ -219,14 +245,14 @@
                     changeState(12);
                                              
                 //Otherwise continue moving forward until distance determined by levy algorithm is calculated. 
-                } else if(timerLevy.read_us() > levyTargetTimeus){
+                } else if(timerLevy.read_us() > levy_target_time_us){
                     flagSetNewLevyTime = 1;
                     piswarm.play_tune("G",1);
                     changeState(12);
                     
                 } else if (flagStationary == 1){
                                
-                    piswarm.forward(BASESPEED);
+                    piswarm.forward(BASE_SPEED);
                     flagStationary = 0;
                 }                
                 
@@ -330,9 +356,9 @@
                         
                         if(flagBeaconIlluminated == 0){
                             if(gv_IRDistances[0] < 100 || gv_IRDistances[1] < 100){
-                                turnDegrees(90);
+                                turnDegrees(-90);
                             } else if (gv_IRDistances[6] < 100 || gv_IRDistances[7] < 100){
-                                turnDegrees(-90);
+                                turnDegrees(90);
                             }
                         }