![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
3rd year group project. Electronic and Electrical Engineering. Heriot-Watt University. This is the code for the mbed for the Automatic Little Object Organiser (ALOO).
Dependencies: MCP23017 TCS3472_I2C WattBob_TextLCD mbed
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);