Revision 32:9a4046224b11, committed 2015-12-17

Comitter:

dreamselec

Date:

Thu Dec 17 17:23:24 2015 +0100

Parent:

31:16e056b9f9e0

Commit message:

Comments and clean up.

Changed in this revision

diff -r 16e056b9f9e0 -r 9a4046224b11 Block.cpp
--- a/Block.cpp	Thu Dec 03 15:50:14 2015 +0000
+++ b/Block.cpp	Thu Dec 17 17:23:24 2015 +0100
@@ -2,7 +2,7 @@
  * Block.cpp
  *
  *  Created on: 16 Nov 2015
- *      Author: chandansiyag
+ *      Author: Chandan Siyag
  */
 
 #include "Block.h"
@@ -18,12 +18,15 @@
 
 }
 
+/// Create a new colour object from passed array of components.
 Colour::Colour(float components[4]){
 	for (int i = 0; i < sizeof(components)/sizeof(*components); i++){
 		this->components[i] = components[i];
 	}
 }
 
+/// Create new colour object from passed colour object.
+/// Copies data, not a new pointer
 Colour::Colour(const Colour& rhs){
 	setRed(rhs.getRed());
 	setBlue(rhs.getBlue());
@@ -31,6 +34,8 @@
 	setAlpha(rhs.getAlpha());
 }
 
+/// Sends description to PC about itself.
+/// Cannot be used here since it doesn't know about pc object. Defined in globals.h
 void Colour::printDescription(){
 //	pc.printf("Red: %.3f, Green: %.3f, Blue: %.3f, Clear: %.3f\n", this->component[0], this->component[1], this->component[2], this->component[3]);
 }
@@ -41,10 +46,9 @@
 	this->averageColour = Colour();
 	this->maxColour = Colour();
 	this->colour = Block::Red;
-//		this->minError = {0, 0, 0, 0};
-//	this->maxError = {0, 0, 0, 0};
 }
 
+/// Create new block object from passed size and colour
 Block::Block(Size size, BlockColour blockColour){
 	this->minColour = Colour();
 	this->averageColour = Colour();
@@ -53,32 +57,29 @@
 	this->size = size;
 }
 
+/// Create new block with size and default colour
 Block::Block(Size size) {
 	// TODO Auto-generated constructor stub
 	this->size = size;
 	this->minColour = Colour();
 	this->maxColour = Colour();
 	this->averageColour = Colour();
-//		this->minError = {0, 0, 0, 0};
-//	this->maxError = {0, 0, 0, 0};
 }
 
+/// Create new block object with passed properties
 Block::Block(Size size, Colour minColour, Colour maxColour, Colour averageColour){
 	this->size = size;
 	this->minColour = Colour(minColour);
 	this->maxColour = Colour(maxColour);
 	this->averageColour = Colour(averageColour);
-//		this->minError = {0, 0, 0, 0};
-//	this->maxError = {0, 0, 0, 0};
 }
 
+/// Copy block object to a new one.
 Block::Block(const Block& rhs){
 	minColour = Colour(rhs.minColour);
 	maxColour = Colour(rhs.maxColour);
 	averageColour = Colour(rhs.averageColour);
 	size = rhs.size;
-//	minError = rhs.minError;
-//	maxError = rhs.maxError;
 }
 
 Block::~Block() {

diff -r 16e056b9f9e0 -r 9a4046224b11 Block.h
--- a/Block.h	Thu Dec 03 15:50:14 2015 +0000
+++ b/Block.h	Thu Dec 17 17:23:24 2015 +0100
@@ -2,7 +2,11 @@
  * Block.h
  *
  *  Created on: 16 Nov 2015
- *      Author: chandansiyag
+ *      Author: Chandan Siyag
+ *
+ *      Defines two classes:
+ *      - Colour: is a colour object used for saving/retriving data from sensor. Can also do simple manipulations.
+ *      - Block: class to save a block data, any new block that passes through, or hazardous block.
  */
 
 #ifndef _block_h_
@@ -55,7 +59,7 @@
 
 class Block {
 public:
-// TODO: Switched
+	// ENUM to help make code readable and reduce mistakes.
 	enum Size {Small = 0, Big = 1};
 	enum BlockColour {Wrong = -1, Red = 0, White = 1, Blue = 2, Green = 3, Orange = 4, Yellow = 5, Black = 6};
 

diff -r 16e056b9f9e0 -r 9a4046224b11 commander.cpp
--- a/commander.cpp	Thu Dec 03 15:50:14 2015 +0000
+++ b/commander.cpp	Thu Dec 17 17:23:24 2015 +0100
@@ -6,13 +6,13 @@
 #include "commander.h"
 
 using namespace std;
-#define _DEBUG_
 
-//const int kCommandBufferSize = 40;
 const int kCommandValueBufferSize = 80;
 const int kObjectBufferSize = 20;
 
+/// All objects that can recieve commands
 string CommandObjectValue [6] = { "mbed", "pc", "colour_sensor", "servos", "port", "break_beam" };
+/// All object(fist column) and their possible commands(respective row).
 string CommandObjectCommandsValue [6][kMaxCommandCount] = {
     {"mbed", "haz-block", "read-current-block", "mode", "sort", "reading-count", "", "", "", ""},
     {"pc", "connect", "disconnect", "", "", "", "", "", "reply-commands", "exit"},
@@ -22,17 +22,19 @@
     {"break_beam", "test", "", "", "", "", "", "", "", ""},
 };
 
+/// Creates new commander object
 Commander::Commander()
 {
     replyCommands = false;
     this->resetVariables();
 }
-
+/// Desctructor
 Commander::~Commander()
 {
 
 }
 
+/// Recieves and decodes the command
 void Commander::decodeCommand(CommandTypeRaw type)
 {
     this->resetVariables();
@@ -46,7 +48,6 @@
     }
 
     if (this->readCommand(this->objectRaw) == false) {
-        //        this->description();
         pc.printf("DEBUG:Invalid command.\n");
         return;
     } else if (connectedToPC == true || (this->typeRaw == Set && this->objectRaw == PC && this->commandIndex[0] == 1)) {
@@ -63,6 +64,7 @@
     return;
 }
 
+/// Get the information about the parsed command in a readable manner.
 //TODO: Fix format
 std::string Commander::description()
 {
@@ -80,6 +82,7 @@
     return str;
 }
 
+/// Reads and assignes the imminent commanding object.
 void Commander::readCommandObject()
 {
     char objectInitiator = '<';
@@ -117,6 +120,8 @@
     return;
 }
 
+/// Reads the command for the previouly read object.
+/// Return true if is a possible command, false otherwise.
 bool Commander::readCommand(CommandObjectRaw objectRaw)
 {
     char nextChar = '\0';
@@ -159,35 +164,25 @@
         }
         string tempCommandStr(commandCharArray[i]);
         string tempValueStr(commandValueArray[i]);
-        //		pc.printf("%s\n", tempCommandStr.c_str());
         int row = this->objectRaw;
-        //        pc.printf("Row: %i\n", this->objectRaw);
         int column = 1;
-        //        pc.printf("Column: %i\n", column);
         for (; column < kMaxCommandCount - 1; column++) {
-            //            pc.printf("%i\n", column);
             if (CommandObjectCommandsValue[row][column] == tempCommandStr) {
-                //                pc.printf("Found matching command.\n");
                 this->command[i] = tempCommandStr;
-                //                pc.printf("%s\n", this->command[i].c_str());
                 this->commandIndex[i] = column;
-                //                pc.printf("%i\n", this->commandIndex[i]);
                 this->commandValue[i] = tempValueStr;
-                //                pc.printf("%s\n", this->commandValue[i].c_str());
-                //                pc.printf("%s\n", this->description().c_str());
                 break;
             }
         }
         if (this->commandIndex[i] == -1) {
-            //            pc.printf("index = -1\n");
             return false;
         }
     }
 
-    //	pc.printf("Returning\n");
     return true;
 }
 
+/// Executes the previously read command with the object.
 void Commander::executeCommand()
 {
     switch (this->objectRaw) {
@@ -349,7 +344,7 @@
     return;
 }
 
-
+// Cleans all the properties, ready to recieved next command.
 void Commander::resetVariables()
 {
     this->object = "";

diff -r 16e056b9f9e0 -r 9a4046224b11 commander.h
--- a/commander.h	Thu Dec 03 15:50:14 2015 +0000
+++ b/commander.h	Thu Dec 17 17:23:24 2015 +0100
@@ -1,6 +1,9 @@
 //
 // commander.h
 // Created by Chandan Siyag on 14/11/2015.
+//
+// Parses and executes commands received from the PC.
+//
 
 #include "globals.h"
 

diff -r 16e056b9f9e0 -r 9a4046224b11 fpga.cpp
--- a/fpga.cpp	Thu Dec 03 15:50:14 2015 +0000
+++ b/fpga.cpp	Thu Dec 17 17:23:24 2015 +0100
@@ -5,6 +5,7 @@
 #include "fpga.h"
 #include "mbed.h"
 
+// Macro to sync MBED and FPGA clocks
 #define CLK() bbClk.write(1); wait(0.000001); bbClk.write(0); wait(0.000001);
 
 DigitalOut bbClk(p11);
@@ -21,23 +22,20 @@
 
 }
 
-/**  Function that checks first IR sensor to detect if block in shoot
-    @return 1 if sensor beam broken, 0 otherwise
- */
+/// Function that checks first IR sensor to detect if block in shoot
+/// @return 1 if sensor beam broken, 0 otherwise
 int FPGA::checkForBlock(){
 	return this->getBeamValue(1);
 }
 
-/**  Function that checks if block is also breaking the second IR sensor.
-    @result 1 if it's large, 0 otherwise
- */
+/// Function that checks if block is also breaking the second IR sensor.
+/// @result 1 if it's large, 0 otherwise
 int FPGA::checkForSize(){
 	return this->getBeamValue(2);
 }
 
-/** Moves the stopping servo in the "hopper" to stop blocks from falling
-    @param position: TBD
- */
+/// No longer used since idea of using top servo was discontinued.
+/// Moves the stopping servo in the "hopper" to stop blocks from falling
 void FPGA::moveStoppingServo(StoppingServoPositions position) {
 	if (position == Stop){
 		bbRst.write(0);
@@ -135,9 +133,7 @@
 	this->stoppingServoPosition = position;
 }
 
-/** Moves the servoe which decides which basket block should go into i.e. bad/good
-    @param positon: TBD
- */
+/// Moves the servoe which decides which basket block should go into i.e. haz or not
 void FPGA::moveSortingServo(SortingServoPositions position){
 	if (position == Haz){
 			// HAZ = 26
@@ -233,6 +229,8 @@
 	this->sortingServoPosition = position;
 }
 
+/// Toggles stopping servo based on current position.
+/// Used in the servo test mode.
 void FPGA::toggleStoppingServo(){
 	if (this->stoppingServoPosition == Go)
 		this->moveStoppingServo(Stop);
@@ -240,6 +238,8 @@
 		this->moveStoppingServo(Go);
 }
 
+/// Toggles sorting servo based on current position
+/// Used in the servo test mode.
 void FPGA::toggleSortingServo() {
 	if (this->sortingServoPosition == Haz)
 		this->moveSortingServo(NonHaz);
@@ -247,23 +247,25 @@
 		this->moveSortingServo(Haz);
 }
 
-/** Reset the stopping servo to default posiotn i.e. not letting blcoks fall.
- */
+/// Place holder function. Since all servo move to their default position after kServoWaitTime
+///Reset the stopping servo to default position
 void FPGA::resetStoppingServo(){
 
 }
-/** Reset the sorting servo to default posiont i.e. falling blocks go in haz-bin
- */
+/// Place holder function. Since all servo move to their default position after kServoWaitTime
+/// Reset the sorting servo to default position
 void FPGA::resetSortingServo(){
 
 }
-/** Reset both servos, simply calls their respective reset methods.
- */
+/// Place holder function. Since all servo move to their default position after kServoWaitTime
+/// Reset both servos, simply calls their respective reset methods.
 void FPGA::resetAllServos(){
 	resetSortingServo();
 	resetStoppingServo();
 }
 
+/// Get beam value i.e. high = 1, low = 0;
+/// Beam number Top = 1, Bottom = 2. USE ENUMS to avoid mistakes.
 int FPGA::getBeamValue(int beamNumber){
 	// Return 0 or 1 value for break beam
 	bbRst.write(0);
@@ -303,6 +305,8 @@
 	return -1;
 }
 
+/// Place holder for more servos that can be attached.
+/// Currently both servo have their own designated methods.
 void FPGA::moveServo(int servoNumber, int position){
 
 }

diff -r 16e056b9f9e0 -r 9a4046224b11 fpga.h
--- a/fpga.h	Thu Dec 03 15:50:14 2015 +0000
+++ b/fpga.h	Thu Dec 17 17:23:24 2015 +0100
@@ -1,6 +1,10 @@
 //
 // fpga.h
 // Created by Chandan Siyag on 14/11/2015.
+//
+// All the things FPGA.
+// Moving servos, getting break beam data.
+
 #include "globals.h"
 
 #ifndef _fpag_h_

diff -r 16e056b9f9e0 -r 9a4046224b11 globals.cpp
--- a/globals.cpp	Thu Dec 03 15:50:14 2015 +0000
+++ b/globals.cpp	Thu Dec 17 17:23:24 2015 +0100
@@ -1,3 +1,7 @@
+//
+// globals.cpp
+// Created by Chandan Siyag on 13/11/2015.
+
 #include "globals.h"
 #include "Block.h"
 #include "fpga.h"
@@ -6,18 +10,20 @@
 #include "WattBob_TextLCD.h"
 
 int kDefaultBaudRate = 19200;
-//TODO: Not let it be constant.
+
 int ColourSensorError = 0.5;
-//SerialBase gParity = SerialBase::None;
+//SerialBase gParity = SerialBase::None, 2 = Even, 3 = Odd
 int gStopBits = 1;
 float gIntegrationTime = 2.5;
 int gToggleServoNumber = 0;
 float currentMinError[3] = {0,0,0};
 float currentMaxError[3] = {0,0,0};
 
+// Intialise gloabl variables
 Block defaultHazBlock = Block(Block::Small, Block::Red);
 Block _HazBlock = Block(defaultHazBlock);
 
+// Default gloabl variables values
 bool connectedToPC = false;
 bool runServoTest = false;
 bool runBreakBeamTest = false;
@@ -32,6 +38,7 @@
 PCModes currentMode = None;
 Controls currentState = Pause;
 
+/// Sets up the default hazardous block
 void DefaultHazBlock(){
 	
 	for (int i = 0; i < 3; i++){ 
@@ -48,24 +55,20 @@
 		_averageRedBlock.components[i] = kAverageRedBlock[i];
 	}
 	
-//	_HazBlock.minColour = Colour(_minRedBlock);
-//	_HazBlock.maxColour = Colour(_maxRedBlock);
-//	_HazBlock.averageColour = Colour(_averageRedBlock);
-//	_HazBlock.size = Block::Small;
-	
 	defaultHazBlock.minColour = Colour(_minRedBlock);
 	defaultHazBlock.maxColour = Colour(_maxRedBlock);
 	defaultHazBlock.averageColour = Colour(_averageRedBlock);
 	defaultHazBlock.size = Block::Small;
 	
 	_HazBlock = Block(defaultHazBlock);
-//	pc.printf(	"VALUE:HazBlock:\n \t Size:%i\n \t Min Colour:%f,%f,%f,%f\n \t Max Colour:%f,%f,%f,%f:VALUE", _HazBlock.size, _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha], _HazBlock.maxColour.components[Colour::Red], _HazBlock.maxColour.components[Colour::Blue], _HazBlock.maxColour.components[Colour::Green], _HazBlock.maxColour.components[Colour::Alpha]);
 }
 
+/// Sends the supplied colours description to the PC
 void printColourDescription(Colour colour){
 	pc.printf("Red: %.3f, Green: %.3f, Blue: %.3f, Clear: %.3f\n", colour.components[0], colour.components[1], colour.components[2], colour.components[3]);
 }
 
+/// Sends the block infomation to the PC.
 void printBlockDescription(Block block){
 	pc.printf("VALUE:Size: %i\n", block.size);
 	printColourDescription(block.minColour);
@@ -75,6 +78,8 @@
 	pc.printf(":VALUE");
 }
 
+/// Reads the switches on the MBED pressed.
+/// Finishes on the Key-Up
 int readSwitches()
 {
     if(i2cport->read_bit(8)) {
@@ -95,12 +100,14 @@
 
 }
 
+/// Sets connecteToPC property to true when PC asks MBED to connect to itself.
 void connectToPC(CommandTypeRaw typeRaw)
 {
     connectedToPC = true;
     pc.printf("INFO:PC connected to MBED.\n");
 }
 
+/// Called when PC tells MBED to disconnect.
 void disconnectToPC(CommandTypeRaw typeRaw)
 {
     pc.printf("INFO:PC disconnected to MBED.\n");
@@ -108,6 +115,8 @@
     currentMode = None;
 }
 
+/// Sends all the information about the hazardous block to the PC.
+/// Or sets the new hazardous block depending on the command type i.e. Set or Query
 void hazBlock(CommandTypeRaw typeRaw)
 {
     if (typeRaw == Set) {
@@ -119,11 +128,14 @@
     }
 }
 
+/// Placeholder function, not used anymore since it's done with hazBlock(!)
+/// Is supposed to send information about next block.
 void getCurrentBlock(CommandTypeRaw typeRaw)
 {
     pc.printf("DEBUG: Getting current block readings\n");
 }
 
+/// Sets colour sensor Integration time set by the PC.
 void setIntegrationTimeTo(float integrationTime)
 {
     gIntegrationTime = integrationTime;
@@ -131,11 +143,14 @@
     pc.printf("DEBUG: Setting integration-time to %.2f.\n", gIntegrationTime);
 }
 
+/// Placeholder function. Done with colour sensor test mode.
+/// Is supposed to send current colour sensor information at regular intervals.
 void previewOnPC(bool on)
 {
     pc.printf("setting preview on pc to %i.\n", on);
 }
 
+/// Start/stop colour sensor test mode.
 void testColourSensor(Controls state){
 	if (state == Start){
 		pc.printf("INFO: Running colour test.\n");
@@ -146,14 +161,16 @@
 	}
 }
 
+/// Sends current colour sensor values.
 void readColourSensor()
 {
     int colourValue[4];
     rgbSensor.getAllColors(colourValue);
-    pc.printf(	"VALEU:Colour Reading:%i,%i,%i,%i\n:VALUE", colourValue[0], colourValue[1], colourValue[2], colourValue[3]);
+    pc.printf(	"VALUE:Colour Reading:%i,%i,%i,%i\n:VALUE", colourValue[0], colourValue[1], colourValue[2], colourValue[3]);
 
 }
 
+/// Enter/exit servo test mode.
 void testServos(Controls state)
 {
     if (state == Start) {
@@ -165,16 +182,19 @@
     }
 }
 
+/// Placeholder function. Servos now are always set to the default position i.e Non Hazardous (upright).
 void resetServos()
 {
     pc.printf("resetting servos.\n");
 }
 
+/// Placeholder functions. The port info is sent every time port is opened, and updated every time anything changes.
 void getPortInfo()
 {
     pc.printf("getting port info.\n");
 }
 
+/// Set the port baud rate to what PC told it to.
 void setPortBaudRate(int baudRate)
 {
     pc.baud(baudRate);
@@ -182,6 +202,7 @@
     pc.printf("DEBUG: Setting port Baud Rate to: %i.\n", baudRate);
 }
 
+/// Sets port parity according to the PC.
 void setPortParity(int parity)
 {
     SerialBase::Parity _parity = static_cast<SerialBase::Parity>(parity);
@@ -190,6 +211,7 @@
     pc.printf("DEBUG: Setting port parity to: %i.\n", parity);
 }
 
+/// Start/pause break beam test mode.
 void testBreakBeams(Controls state){
 	if (state == Start){
 		pc.printf("INFO: Running break beam test.\n");

diff -r 16e056b9f9e0 -r 9a4046224b11 globals.h
--- a/globals.h	Thu Dec 03 15:50:14 2015 +0000
+++ b/globals.h	Thu Dec 17 17:23:24 2015 +0100
@@ -1,19 +1,22 @@
 
-
-
+//
+// globals.h
+// Created by Chandan Siyag on 13/11/2015.
+//
+// Includes all the functions, variables, objects that need to be accessed accross the classes/files.
 #ifndef _globals_h_
 #define _globals_h_
 
 #include "Block.h"
 #include "mbed.h"
-//#include "MCP23017.h"
-//#include "fpga.h"
 
+// Forward declared classes
 class FPGA;
 class TCS3472_I2C;
 class MCP23017;
 class WattBob_TextLCD;
 
+// External global variables
 extern mbed::Serial pc;
 extern MCP23017 *i2cport;
 extern int kDefaultBaudRate;
@@ -32,19 +35,23 @@
 extern int hazReadingCount;
 extern bool setNewHazBlock;
 
+// Constants
 const int kCommandBufferSize = 80;
 const int kSmallBufferSize = 5;
 const float kServoWait = 0.20;
 
+// More external globals vars
 extern float currentMinError[3];
 extern float currentMaxError[3];
 extern bool pcModeChanged;
 #define FOREVER for(;;)
 
+// More constants and enums
 const char CommandTypeValue [3] = {'!', '?', ':'};
 const char kCommandTerminator = ';';
 enum CommandTypeRaw { InvalidType = -1, Set = 0, Query = 1, Reply = 2 };
 
+// Red block values
 const float kMinRedBlock[4] = {4.812, 2.286, 1.316, 1};
 const float kMaxRedBlock[4] = {5.474, 3.105, 1.429, 1};
 const float kAverageRedBlock[4] = {5.25134482758621, 2.85965517241379, 1.35858620689655, 1};
@@ -82,20 +89,25 @@
 		{ 0, 0, 0, 1 }
 };
 
+// External global objects
 extern Block defaultHazBlock;
 extern Block _HazBlock;
 extern FPGA *fpga;
 
 extern TCS3472_I2C rgbSensor;
 
+// Enums to help make code more readable and easier to edit
 enum Servos {Stopping = 1, Sorting = 2};
 enum StoppingServoPositions {Stop = 0, Go = 1};
 enum SortingServoPositions {NonHaz = 0, Haz = 1};
 enum Controls { Start = 0, Pause = 1};
 enum PCModes { None = -1, Normal = 0, Maintanence = 1};
 
+// External variables
 extern PCModes currentMode;
 extern Controls currentState;
+
+// Global functions
 void DefaultHazBlock();
 
 int readSwitches();

diff -r 16e056b9f9e0 -r 9a4046224b11 main.cpp
--- a/main.cpp	Thu Dec 03 15:50:14 2015 +0000
+++ b/main.cpp	Thu Dec 17 17:23:24 2015 +0100
@@ -46,6 +46,7 @@
 
 extern PCModes currentMode;
 
+/// Forward declared functions
 void initInternal();
 void initPort(int baudRate=kDefaultBaudRate);
 void printPCDetectedText();
@@ -62,12 +63,16 @@
 void runInColourSensorTestMode();
 void newHazBlockMode();
 
+
+/// The main program function. Simply put just a infinite for loop. Not so simply put multiple infinite for loops
 int main()
 {
+	// Initialization
     initInternal();
     initPort();
 
     for (;;) {
+    	// Show option if PC not detected.
         if (connectedToPC == false) {
             pc.printf("DEBUG: PC did not responded.\n");
             lcd->cls();
@@ -79,15 +84,13 @@
             lcd->printf("2: Connect to PC");
         }
 
-//		wait(0.03);
-
         int selection = 0;
         do {
             myLED4 = selection;
             selection = readSwitches();
         } while (selection != 1 && selection != 2 && connectedToPC == false);
-//		lcd->cls();
-        D_LEDS_OFF();
+        turnOffBottomLEDs();
+
         if (selection == 1) {
             // User selected op 1: Start sorting autonomously.
             i2cport->write_bit(1, 12);
@@ -122,6 +125,7 @@
             }
         }
 
+        // Connect to PC
         if (selection == 2 || connectedToPC == true) {
             wait(0.1);
             for (;;) {
@@ -129,6 +133,7 @@
 
                 i2cport->write_bit(1, 15);
                 int abortOperation = false;
+                // Probe for PC since it's not already connected.
                 if (connectedToPC == false)
                     pc.printf(":<pc>connect;");
                 while (connectedToPC == false && abortOperation == false) {
@@ -143,6 +148,7 @@
                 displayPCStatus();
 
                 while (abortOperation == false && connectedToPC == true) {
+                	// Maintenance mode states
                     if (currentMode == Maintanence) {
                         displayPCStatus();
                         while (currentMode == Maintanence) {
@@ -183,7 +189,6 @@
                                     } else if (button == 4) {
                                         currentMode = None;
                                         pc.printf(":<mbed>mode=none;");
-                                        //                                        goto setModeNone;
                                         break;
                                     }
                                 }
@@ -194,8 +199,8 @@
                                 lcd->printf("Sorting mode...");
                                 while (currentState == Start && currentMode == Normal) {
                                     if (waitForBlock() == false) {
+                                    	// Sorting paused from MBED button, tell PC about it.
                                         if (connectedToPC == true && currentState != Pause && currentMode == Normal) {
-                                            // TODO: Tell PC to update UI if aborted from MBED.
                                             pc.printf(":<mbed>sort=pause;");
                                             currentState = Pause;
                                             continue;
@@ -214,7 +219,7 @@
                         }
                     } else if (currentMode == None) {
 setModeNone:
-                        D_LEDS_OFF();
+						turnOffBottomLEDs();
                         i2cport->write_bit(1,15);
                         displayPCStatus();
                         while (currentMode == None && abortOperation == false && connectedToPC == true) {
@@ -229,7 +234,7 @@
                         }
                     }
                 }
-
+                // Return to Main Menu
                 if (abortOperation == true ) {
                     connectedToPC = false;
                     D_LEDS_OFF();
@@ -294,17 +299,20 @@
     return false;
 }
 
+// Sort current block; called just after block is detected.
 void sortBlock()
 {
     pc.printf("BLOCK: Sorting block");
     myLED1 = 0;
     myLED2 = 1;
-    // Cannot Abort any longer. Block is inserted.
+    // Cannot Abort any longer.
     // Detach rx interrupt until block processed.
     NVIC_DisableIRQ(UART1_IRQn);
+
     fpga->moveSortingServo(Haz);
     fpga->moveStoppingServo(Go);
 
+    // Take 3 readings and use the average.
     int colourValues[4];
     int averageColourValues[4] = {0, 0, 0, 0};
     int numberOfReadings = 3;
@@ -320,8 +328,8 @@
 
     lastBlockHaz = false;
     lastBlockHaz = checkColour(averageColourValues);
-//	lastBlockHaz = checkColour(colourValues);
 
+    // Tell FPGA to move servo, optimises the process.
     if (!lastBlockHaz) {
         fpga->moveSortingServo(NonHaz);
     }
@@ -330,10 +338,8 @@
     int blockSize;
     while (fpga->checkForBlock() == 0) {  }
     blockSize = fpga->checkForSize();
+    // Size and colour fit the criteria of the hazardous block.
     if (blockSize == HazBlock->size && lastBlockHaz) {
-        //        fpga->moveSortingServo(Haz);
-        //        fpga->moveStoppingServo(Go);
-        //        blockSize = HazBlock->size;
         while(fpga->getBeamValue(Bottom) == 1) {}
         wait(kServoWait);
         fpga->moveStoppingServo(Stop);
@@ -343,14 +349,12 @@
     fpga->moveSortingServo(NonHaz);
     while(fpga->checkForSize()) {}
 
+    // Print detailed information about the block on the PC.
     if (connectedToPC) {
         for (int i = 0; i < 3; i++) {
             pc.printf("DEBUG:Percentage Error: %.5f. Passed?: %i\n", percentageError[i], isBetweenHazValues[i]);
-//            if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinRedError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxRedError[i] * errorMultiplier))
-//                pc.printf("DEBUG:%i Pass.\n", i);
         }
         pc.printf("BLOCK:Size:%i,Red:%.3f,Green:%.3f,Blue:%.3f,Haz:%i, Offsetred:%.3f, Offsetgreen:%.3f, Offsetblue:%.3f;", blockSize, adjustedValues[0], adjustedValues[1], adjustedValues[2], lastBlockHaz, percentageError[0], percentageError[1], percentageError[2]);
-        //        pc.printf("VALUE:Size:%i,Red:%i,Green:%i,Blue:%i,Clear:%i\n:VALUE", blockSize, colourValues[0], colourValues[1], colourValues[2], colourValues[3], lastBlockHaz);
     }
 
     // Re-Attach rx interrupt
@@ -362,16 +366,18 @@
     return;
 }
 
+/// PC Read interrupt
 /// Called every-time it receives an char from PC.
 void Rx_interrupt()
 {
     recievingResponse = true;
     char interruptChar = pc.getc();
-    // Uncomment to Echo to USB serial to watch data flow
+    // Uncomment to echo to USB serial to watch data flow
     //    pc.putc(interruptChar);
 
     NVIC_DisableIRQ(UART1_IRQn);
 
+    // Only start parsing command if it start with three command types, otherwise ignore.
     if (interruptChar == CommandTypeValue[Query]) {
         commander->decodeCommand(Query);
     } else if (interruptChar == CommandTypeValue[Set]) {
@@ -384,6 +390,8 @@
     recievingResponse = false;
 }
 
+/// Initialize internal parts.
+/// Colour sensor, LCD, and Servos
 void initInternal()
 {
     myLED1 = 1;
@@ -403,6 +411,7 @@
     return;
 }
 
+/// Initilise the serial port
 void initPort(int baudRate)
 {
     myLED3 = 1;
@@ -417,6 +426,7 @@
     return;
 }
 
+/// Check if the passed colour values fit the hazardous block values.
 bool checkColour(int colourValues[])
 {
     for (int i = 0; i < 3; i++) {
@@ -425,26 +435,6 @@
     memset(adjustedValues, 0, sizeof(adjustedValues));
     memset(percentageError, 0, sizeof(percentageError));
 
-    /* Harcoded working
-    	for (int i = 0; i < 3; i++) {
-    		adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
-    		percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
-
-    		if ((percentageError[i] < 0 && std::abs(percentageError[i]) < kMinError[i] * 2) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < kMaxError[i] * 2)) {
-    			isBetweenHazValues[i] = true;
-    		}
-    	}
-    */
-    /*
-    for (int i = 0; i < 3; i++) {
-        adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
-        percentageError[i] = (adjustedValues[i] - kAverageRedBlock[i]) / kAverageRedBlock[i];
-    	// Don't forget to call DefaultHazBlock in init otherwise currentMin/MaxErr won't be set.
-        if ((percentageError[i] < 0 && std::abs(percentageError[i]) < currentMinError[i] * errorMultiplier) || percentageError[i] == 0 || (percentageError[i] > 0 && percentageError[i] < currentMaxError[i] * errorMultiplier)) {
-            isHazColour[i] = true;
-        }
-    }
-    */
     for (int i = 0; i < 3; i++) {
         adjustedValues[i] = (float)colourValues[i]/(float)colourValues[3];
         percentageError[i] = (adjustedValues[i] - kAverageValues[HazBlock->colour][i]) / kAverageValues[HazBlock->colour][i];
@@ -465,12 +455,13 @@
     return isHazBlock;
 }
 
+/// Setting new hazardous block mode, from maintenance mode.
 void newHazBlockMode()
 {
 trySetHazBlockAgain:
     pc.printf("NHZB:Size:%i,Colour:%i;", _HazBlock.size, _HazBlock.colour);
     fpga->moveSortingServo(Haz);
-//        pc.printf("INFO:Setting new haz block.\n");
+
     int lowerBeam = 0;
     int higherBeam = 0;
     int colourValues[6][4];
@@ -487,7 +478,6 @@
         higherBeam = fpga->getBeamValue(Top);
         if (readSwitches() == 4) {
             if (displayAbortDialog()) {
-                //TODO: tell pc
                 pc.printf(":<mbed>haz-block=pause;");
                 pc.printf("INFO: Operation aborted form MBED.\n");
                 fpga->moveSortingServo(NonHaz);
@@ -588,7 +578,6 @@
         } while (button != 2);
     }
 
-    // Point and literal might not sync...
     _HazBlock.size = static_cast<Block::Size>(blockSize);
     _HazBlock.colour = detectedColour;
 
@@ -600,6 +589,7 @@
     fpga->moveSortingServo(NonHaz);
 }
 
+/// Prints info on LCD about PC
 void printPCDetectedText()
 {
     lcd->cls();
@@ -610,6 +600,7 @@
     initPort();
 }
 
+/// Return true if user aborts using the dialog. Requires confimation.
 bool displayAbortDialog()
 {
     while (i2cport->read_bit(11) == 1) {}
@@ -627,14 +618,13 @@
 
     D_LEDS_OFF();
     if (reply == 1) {
-        //		while (i2cport->read_bit(8)) {  }
         return true;
     } else {
-        //		while (i2cport->read_bit(9) || i2cport->read_bit(10) || i2cport->read_bit(11)) {  }
         return false;
     }
 }
 
+/// Prints servo info on the LCD in the test mode
 void printServoInfoOnLCD()
 {
     lcd->cls();
@@ -651,6 +641,7 @@
         lcd->printf("2:Bottom: NonHaz");
 }
 
+/// Sends servo test mode info on the pc
 void printServoInfoOnPC()
 {
     if (fpga->stoppingServoPosition == Stop)
@@ -664,6 +655,7 @@
         pc.printf(":<servos>2=Haz;");
 }
 
+/// Runs the servo test mode
 void runInServoTestMode()
 {
     pc.printf("VALUES:Testing servos.\n Stopping servo:\n\tStop:%i, Go: %i\n Sorting servo:\n\tHaz:%i, NonHaz:%i\n:VALUES", Stop, Go, Haz, NonHaz);
@@ -717,6 +709,7 @@
     return;
 }
 
+/// Print break bream test mode info on the LCDs
 void printBeamInfoOnLCD()
 {
     lcd->cls();
@@ -726,11 +719,13 @@
     lcd->printf("On:High, Off:Low");
 }
 
+/// Sends break bream test info to the PC
 void printBeamInfoOnPC(int topBeam, int bottomBeam)
 {
     pc.printf(":<break_beam>2=%i,1=%i;", topBeam, bottomBeam);
 }
 
+/// Runs the break beam test
 void runInBreakBeamTestMode()
 {
     turnOffTopLEDs();
@@ -776,6 +771,7 @@
     return;
 }
 
+/// Prints colour sensor test info on the LCD
 void printColourSensorInfoOnLCD(int colourValues[])
 {
     float weightedValues[4];
@@ -791,11 +787,13 @@
     lcd->printf("Test mode");
 }
 
+/// Sends colour sensor values to the PC
 void printColourSensorInfoOnPC(int colourValues[])
 {
     pc.printf(":<colour_sensor>red=%i,green=%i,blue=%i,clear=%i;", colourValues[0], colourValues[1], colourValues[2], colourValues[3]);
 }
 
+/// Runs the colour sensor test mode
 void runInColourSensorTestMode()
 {
     turnOffTopLEDs();
@@ -886,6 +884,7 @@
     return;
 }
 
+/// Displays 'Waiting...' on the LCD. Used several times so simpler to create a funciton
 void displayWaitingLine()
 {
     lcd->cls();
@@ -894,6 +893,7 @@
     lcd->locate(1,0);
 }
 
+/// Displays current PC status on the LCD.
 void displayPCStatus()
 {
     lcd->cls();
@@ -914,6 +914,7 @@
     i2cport->write_bit(1,15);
 }
 
+/// Turns off all the top LEDs
 void turnOffTopLEDs()
 {
     myLED1 = 0;
@@ -922,6 +923,7 @@
     myLED4 = 0;
 }
 
+/// Turns off all the bottom swithc LEDs
 void turnOffBottomLEDs()
 {
     i2cport->write_bit(0, 12);