Diff: main.cpp

Revision:

32:9a4046224b11

Parent:

31:16e056b9f9e0

--- 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);