Diff: main.cpp

Revision:

10:16ba52f8e025

Parent:

9:dc8f155b71c8

Child:

11:0fe833f8a1ab

diff -r dc8f155b71c8 -r 16ba52f8e025 main.cpp
--- a/main.cpp	Mon Nov 16 23:44:44 2015 +0000
+++ b/main.cpp	Wed Nov 18 16:09:52 2015 +0000
@@ -25,7 +25,7 @@
 MCP23017 *i2cport;
 WattBob_TextLCD *lcd;
 
-TCS3472_I2C rgb_sensor(p28, p27);
+TCS3472_I2C rgbSensor(p28, p27);
 Serial      pc(USBTX, USBRX);
 uint8_t     rxBuffer[kSmallBufferSize + 1];
 int 		rxIndex = 0;
@@ -41,95 +41,122 @@
 void detectPC();
 void initInternal();
 void initPort(int baudRate=kDefaultBaudRate);
-int readSwitches();
 void printPCDetectedText();
-bool displayAboardDialog();
+bool displayAbortDialog();
 void Rx_interrupt();
+void sortBlock(int colourValues[]);
+void runInServoTestMode();
 
+void displayWaitingLine(){
+	lcd->cls();
+	lcd->locate(0,0);
+    lcd->printf("Waiting...");
+    lcd->locate(1,0);
+}
 
 int main()
 {
-	initInternal();
-	initPort();
-	srand((unsigned)time(NULL));
-	U_LEDS_OFF();
-	lcd->cls();
-	myLED1 = 1;
+    initInternal();
+    initPort();
+    srand((unsigned)time(NULL));
+    U_LEDS_OFF();
+    lcd->cls();
+    myLED1 = 1;
+    
+    rgbSensor.enablePowerAndRGBC();
+    rgbSensor.setIntegrationTime(100);
 
-	// Create a serial intereput for RxIrq so when PC is connected it sends '$' to tell MBED it's there.
-	// https://developer.mbed.org/cookbook/Serial-Interrupts
-	pc.attach(&Rx_interrupt, Serial::RxIrq);
-
-	for (;;) {
-		lcd->cls();
-		i2cport->write_bit(1, 12);
-		lcd->printf("1: Start sorting.");
-		LCDSL();
-		i2cport->write_bit(1,13);
-		lcd->printf("2: Connect to PC");
+    // Create a serial intereput for RxIrq so when PC is connected it sends '$' to tell MBED it's there.
+    // https://developer.mbed.org/cookbook/Serial-Interrupts
+    pc.attach(&Rx_interrupt, Serial::RxIrq);
 
-		int selection = 0;
-		do {
-			myLED4 = selection;
-			selection = readSwitches();
-		} while (selection != 1 && selection != 2 && connectedToPC == false);
-		D_LEDS_OFF();
-		if (selection == 1) {
-			// User selected op 1: Start sorting autonomously.
-			i2cport->write_bit(1, 12);
-			lcd->cls();
-			LCDFL();
-			lcd->printf("Starting sorting");
-			wait(0.5);
-			D_LEDS_OFF();
-			
-			for(;;) {
-				lcd->cls();
-				lcd->printf("Waiting...");
-				
-				i2cport->write_bit(1, 15);
-				bool aboardOperation = false;
+    for (;;) {
+        lcd->cls();
+        i2cport->write_bit(1, 12);
+        lcd->printf("1: Start sorting.");
+        LCDSL();
+        i2cport->write_bit(1,13);
+        lcd->printf("2: Connect to PC");
+        
+        fpga->moveStoppingServo(Stop);
+        fpga->moveSortingServo(NonHaz);
+
+        int selection = 0;
+        do {
+            myLED4 = selection;
+            selection = readSwitches();
+        } while (selection != 1 && selection != 2 && connectedToPC == false);
+        D_LEDS_OFF();
+        if (selection == 1) {
+            // User selected op 1: Start sorting autonomously.
+            i2cport->write_bit(1, 12);
+            lcd->cls();
+            LCDFL();
+            lcd->printf("Starting sorting");
+            wait(0.5);
+            D_LEDS_OFF();
+
+            bool abortOperation = false;
+            for(;;) {
+                displayWaitingLine();
+
+                i2cport->write_bit(1, 15);
+
 
-				int blockInserted = 0;
-				// Wait until a block is breaking the beam, or button 4 is pressed to aboard.
-				do {
-					blockInserted = fpga->checkForBlock();
-					if (i2cport->read_bit(11)) {
-						aboardOperation = displayAboardDialog();
-						// Cancel the aboard
-						if (aboardOperation == false) {
-							lcd->cls();
-							LCDFL();
-							lcd->printf("Waiting...");
-							i2cport->write_bit(1, 15);
-						}
-					}
-				} while (aboardOperation == false && blockInserted != 1);
+                int blockInserted = 0;
+                // Wait until a block is breaking the beam, or button 4 is pressed to abort.
+                do {
+                    blockInserted = fpga->checkForBlock();
+                    myLED4 = blockInserted;
+                    if (i2cport->read_bit(11)) {
+                        abortOperation = displayAbortDialog();
+                        // Cancel the Abort
+                        if (abortOperation == false) {
+                            displayWaitingLine();
+                            lcd->printf("for Block");
+                            i2cport->write_bit(1, 15);
+                        }
+                    }
+                } while (abortOperation == false && blockInserted != 1 && connectedToPC == false);
+
+                // Break and return to main menu i.e. Start Op, or Connect to PC.
+                if (abortOperation == true || connectedToPC == true) {
+                    D_LEDS_OFF();
+                    break;
+                }
+				
+                // Cannot Abort any longer. Block is inserted.
+                // Detach rx interrupt until block processed.
+                NVIC_DisableIRQ(UART1_IRQn);
 
-				// Break and return to main menu i.e. Start Op, or Connect to PC.
-				if (aboardOperation == true) {
-					D_LEDS_OFF();
-					break;
-				}
-
-				// Cannot aboard any longer. Block is inserted.
-				// Detach rx interrupt until block processed.
-				NVIC_DisableIRQ(UART1_IRQn);
-
-				int blockSize = fpga->checkForSize();
+                int blockSize = 0;
+                blockSize = fpga->checkForSize();
+                myLED3 = blockSize;
 
-				if (blockSize == HazBlock->size) {
-					//                        detectColour()
-					lcd->cls();
-					lcd->printf("Detecting Colour");
-				}
+                if (blockSize == 0) {
+                    //                        detectColour()
+                    int colourValues[4];
+                    displayWaitingLine();
+//                    rgbSensor.getAllColors(colourValues);
+                    lcd->cls();
+                    lcd->locate(0,0);
+                    lcd->printf("Sorting");
+                    sortBlock(colourValues);
+                }else {
+                	fpga->moveStoppingServo(Go);
+                	while(fpga->checkForSize()){}
+                	fpga->moveStoppingServo(Stop);
+                }
+				
+				myLED3 = 0;
+                // Re-Attach rx interrupt
+                NVIC_EnableIRQ(UART1_IRQn);
+            }
+//			if (abortOperation == true || connectedToPC == true){ break; }
+        }
 
-				// Re-Attach rx interrupt
-				NVIC_EnableIRQ(UART1_IRQn);
-			}
-
-		} else if (selection == 2 || connectedToPC == true) {
-			for (;;){
+        if (selection == 2 || connectedToPC == true) {
+            for (;;) {
                 if (!connectedToPC) {
                     i2cport->write_bit(1, 13);
                     // Wait for PC to send '!<pc>connect;' command.
@@ -141,119 +168,191 @@
                     LCDSL();
                     lcd->printf("4: Main Menu");
                 }
-				i2cport->write_bit(1, 15);
-				int aboardOperation = false;
-				while (connectedToPC == false && aboardOperation == false){
-					aboardOperation = readSwitches() == 4;
-				}
-				if (aboardOperation == true) { D_LEDS_OFF(); break; }
-				else if (connectedToPC == true){
-					lcd->cls();
-					LCDFL();
-					lcd->printf("Connected to PC");
-					LCDSL();
-					lcd->printf("4: Disconnect.");
-				}
+                i2cport->write_bit(1, 15);
+                int abortOperation = false;
+                while (connectedToPC == false && abortOperation == false) {
+                    abortOperation = readSwitches() == 4;
+                }
+                if (abortOperation == true) {
+                    D_LEDS_OFF();
+                    break;
+                } else if (connectedToPC == true) {
+                    lcd->cls();
+                    LCDFL();
+                    lcd->printf("Connected to PC");
+                    LCDSL();
+                    lcd->printf("4: Disconnect.");
+                }
 
-				while (aboardOperation == false && connectedToPC == true){
-					aboardOperation = readSwitches() == 4;
-				}
-	
-				if (aboardOperation == true ) { D_LEDS_OFF(); break; }
-			}
-		}
+                while (abortOperation == false && connectedToPC == true) {
+					if (runServoTest == true){
+                    	runInServoTestMode();
+                    	lcd->cls();
+                    	lcd->locate(0,0);
+                    	lcd->printf("Connected to PC");
+                    	lcd->locate(1,0);
+                    	lcd->printf("4:Disconnect");
+                    	i2cport->write_bit(1,15);
+                    }
+                    abortOperation = readSwitches() == 4;
+                }
 
-	}
+                if (abortOperation == true ) {
+                    connectedToPC = false;
+                    D_LEDS_OFF();
+                    break;
+                }
+            }
+        }
+
+    }
 }
 
 /// Called every-time it receives an char from PC.
-void Rx_interrupt(){
-	char interruptChar = pc.getc();
-	// Uncomment to Echo to USB serial to watch data flow
-        pc.putc(interruptChar);
+void Rx_interrupt()
+{
+    char interruptChar = pc.getc();
+    // Uncomment to Echo to USB serial to watch data flow
+//    pc.putc(interruptChar);
 
-	NVIC_DisableIRQ(UART1_IRQn);
+    NVIC_DisableIRQ(UART1_IRQn);
 
-	if (interruptChar == CommandTypeValue[Query]){
-		commander->decodeCommand(Query);
-	}else if (interruptChar == CommandTypeValue[Set]){
-		commander->decodeCommand(Set);
-	}else if (interruptChar== CommandTypeValue[Reply]){
-		commander->decodeCommand(Reply);
-	}
+    if (interruptChar == CommandTypeValue[Query]) {
+        commander->decodeCommand(Query);
+    } else if (interruptChar == CommandTypeValue[Set]) {
+        commander->decodeCommand(Set);
+    } else if (interruptChar== CommandTypeValue[Reply]) {
+        commander->decodeCommand(Reply);
+    }
 
-	NVIC_EnableIRQ(UART1_IRQn);
+    NVIC_EnableIRQ(UART1_IRQn);
 }
 
 void initInternal()
 {
-	i2cport = new MCP23017(p9, p10, 0x40);
-	lcd = new WattBob_TextLCD(i2cport);
-	myLED1 = 1;
-	BACKLIGHT_ON(i2cport);
-	lcd->cls();
-	LCDFL();
-	lcd->printf("Initilizing...");
-	myLED2 = 1;
-	return;
+    i2cport = new MCP23017(p9, p10, 0x40);
+    lcd = new WattBob_TextLCD(i2cport);
+    myLED1 = 1;
+    BACKLIGHT_ON(i2cport);
+    lcd->cls();
+    LCDFL();
+    lcd->printf("Initilizing...");
+    myLED2 = 1;
+    return;
 }
 
 void initPort(int baudRate)
 {
-	myLED3 = 1;
-	pc.baud(baudRate);
-	pc.format(8, SerialBase::None, gStopBits);
-	myLED4 = 1;
-	wait (0.1);
-	return;
+    myLED3 = 1;
+    pc.baud(baudRate);
+    pc.format(8, SerialBase::None, gStopBits);
+    myLED4 = 1;
+    wait (0.1);
+    return;
 }
 
-int readSwitches()
-{
-
-	if(i2cport->read_bit(8)) {
-		return 1;
-	} else if (i2cport->read_bit(9)) {
-		return 2;
-	} else if (i2cport->read_bit(10)) {
-		return 3;
-	} else if (i2cport->read_bit(11)) {
-		return 4;
-	} else {
-		return 0;
+void sortBlock(int colourValues[]){
+	bool isHazColour[4] = {false, false, false, false };
+	
+	for (int i = 0; i < 4; i++){
+		if (colourValues[i] < HazBlock->maxColour.components[i] && colourValues[i] > HazBlock->minColour.components[i]){
+			isHazColour[i] = true;
+		}
 	}
-
+	
+	fpga->moveSortingServo(Haz);
+	fpga->moveStoppingServo(Go);
+	while(fpga->checkForBlock()) {}
+	fpga->moveStoppingServo(Stop);
+	wait(0.5);
+	fpga->moveSortingServo(NonHaz);
+	return;
+	
+	bool isHazBlock = false;
+	
+	if (isHazColour[0] && isHazColour[1] && isHazColour[2] && isHazColour[3]){
+		fpga->moveSortingServo(Haz);
+		fpga->moveStoppingServo(Go);
+		while(fpga->checkForBlock()) {}
+		fpga->moveStoppingServo(Stop);
+		fpga->moveSortingServo(NonHaz);
+	}else {
+		fpga->moveStoppingServo(Go);
+		while(fpga->checkForBlock()) {}
+		fpga->moveStoppingServo(Stop);
+	}
 }
 
 void printPCDetectedText()
 {
-	lcd->cls();
-	LCDFL();
-	lcd->printf("Detected PC.");
-	LCDSL();
-	lcd->printf("Connecting");
-	initPort();
+    lcd->cls();
+    LCDFL();
+    lcd->printf("Detected PC.");
+    LCDSL();
+    lcd->printf("Connecting");
+    initPort();
+}
+
+bool displayAbortDialog()
+{
+    while (i2cport->read_bit(11) == 1) {}
+    i2cport->write_bit(1, 12);
+
+    lcd->cls();
+    LCDFL();
+    lcd->printf("Abort?");
+    LCDSL();
+    lcd->printf("1:YES, 2,3,4:NO");
+    int reply = 0;
+    do {
+        reply = readSwitches();
+    } while(reply == 0);
+
+    D_LEDS_OFF();
+    if (reply == 1) {
+        return true;
+    } else {
+        return false;
+    }
 }
 
-bool displayAboardDialog()
-{
-	while (i2cport->read_bit(11) == 1){}
-	i2cport->write_bit(1, 12);
+void printServoInfoOnLCD(){
+    lcd->cls();
+    lcd->locate(0,0);
+    lcd->printf("1: Stop servo:%i", fpga->stoppingServoPosition);
+    lcd->locate(1,0);
+    lcd->printf("2: Sort servo:%i", fpga->sortingServoPosition);
+}
+
+void runInServoTestMode(){
+	pc.printf("Testing servos.\n Stopping servo:\n\tStop:%i, Go: %i\n Sorting servo:\n\tHaz:%i, NonHaz:%i", Stop, Go, Haz, NonHaz);
+	printServoInfoOnLCD();
+	int finished = false;
 	
-	lcd->cls();
-	LCDFL();
-	lcd->printf("Aboard?");
-	LCDSL();
-	lcd->printf("1:YES, 2,3,4:NO");
-	int reply = 0;
-	do {
-		reply = readSwitches();
-	} while(reply == 0);
-
-	D_LEDS_OFF();
-	if (reply == 1) {
-		return true;
-	} else {
-		return false;
-	}
+	i2cport->write_bit(1, 12);
+	i2cport->write_bit(1, 13);
+	i2cport->write_bit(1, 15);
+	int button = 0;
+    do {
+    	button = readSwitches();
+    	if (button == 1){
+    		fpga->toggleStoppingServo();
+    		printServoInfoOnLCD();
+    		wait(0.5);
+    	}else if (button == 2) {
+    		fpga->toggleSortingServo();
+    		printServoInfoOnLCD();
+    		wait(0.5);
+    	}
+    	finished = button == 4;
+    	button = 0;
+    } while (finished == false);
+    runServoTest = false;
+    D_LEDS_OFF();
+    lcd->cls();
+    lcd->locate(0,0);
+    lcd->printf("Done servo test");
+    wait(0.5);
+    lcd->cls();
+    return;
 }