Diff: globals.cpp

Revision:

10:16ba52f8e025

Parent:

9:dc8f155b71c8

Child:

11:0fe833f8a1ab

--- a/globals.cpp	Mon Nov 16 23:44:44 2015 +0000
+++ b/globals.cpp	Wed Nov 18 16:09:52 2015 +0000
@@ -1,80 +1,139 @@
 #include "globals.h"
 #include "Block.h"
 #include "fpga.h"
+#include "TCS3472_I2C.h"
+#include "MCP23017.h"
+#include "WattBob_TextLCD.h"
 
 int kDefaultBaudRate = 19200;
+int ColourSensorError = 25;
 //SerialBase gParity = SerialBase::None;
 int gStopBits = 1;
 
 Block _HazBlock = Block(kDefaultHazBlock);
 
 bool connectedToPC = false;
+bool runServoTest = false;
 
-void connectToPC(CommandTypeRaw typeRaw){
-	pc.printf("pc told to connect\n");
-	connectedToPC = true;
+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 disconnectToPC(CommandTypeRaw typeRaw){
-	pc.printf("pc told to disconnect\n");
-	connectedToPC = false;
+void connectToPC(CommandTypeRaw typeRaw)
+{
+    pc.printf("pc told to connect\n");
+    connectedToPC = true;
+}
+
+void disconnectToPC(CommandTypeRaw typeRaw)
+{
+    pc.printf("pc told to disconnect\n");
+    connectedToPC = false;
 }
 
-void hazBlock(CommandTypeRaw typeRaw){
-	if (typeRaw == Set) {
-		pc.printf("setting new haz block.\n");
-		pc.printf("Send 'y' when block is inserted.\n");
-		int lowerBeam = 0;
-		char readyChar = '\0';
-		do{
-			readyChar = pc.getc();
-			lowerBeam = fpga->checkForBlock();
-			if (readyChar == 'y' && lowerBeam == 0){
-				pc.printf("No block detected by beam\nTry again.");
-			}
-		} while (readyChar != 'y' || lowerBeam != 0);
+void hazBlock(CommandTypeRaw typeRaw)
+{
+    if (typeRaw == Set) {
+        pc.printf("setting new haz block.\n");
+//        pc.printf("Send 'y' when block is inserted.\n");
+        int lowerBeam = 0;
+        int higherBeam = 0;
+        char readyChar = '\0';
+        
+        do { higherBeam = fpga->checkForSize(); 
+        } while (higherBeam != 1);
+        
+        int colourValue[4];
+        rgbSensor.getAllColors(colourValue);
+        
+        do {
+//			readyChar = pc.getc();
+            lowerBeam = fpga->checkForBlock();
+            // TODO: Check beam reading
+//			if (readyChar != 'y'){
+//				pc.printf("No block detected by beam\nTry again.");
+//			}
+        } while (lowerBeam != 1);
+        higherBeam = fpga->checkForSize();
+
+        
+
+
+        _HazBlock = Block();
+        if (higherBeam == 1)
+            _HazBlock.size = Block::Big;
+        else if (higherBeam == 0)
+            _HazBlock.size = Block::Small;
+        for (int i = 0; i < 4; i++) {
+            _HazBlock.minColour.components[i] = colourValue[i] - ColourSensorError;
+            _HazBlock.maxColour.components[i] = colourValue[i] + ColourSensorError;
+        }
+        fpga->moveSortingServo(Haz);
+        fpga->moveStoppingServo(Go);
+        while (fpga->checkForBlock()) {}
+        fpga->moveStoppingServo(Go);
+        fpga->moveSortingServo(NonHaz);
 		
-		int higherBeam = 0;
-		higherBeam = fpga->checkForSize();
-		
-			_HazBlock = Block();
-			if (higherBeam == 1)
-				_HazBlock.size = Block::Big;
-			else if (higherBeam == 0)
-				_HazBlock.size = Block::Small;
-				
-			
-			
-	}else if (typeRaw == Query){
-		pc.printf(	"HazBlock:\n \t Size:%i\n \t Min Colour:%i,%i,%i,%i\n \t Max Colour:TBA\n ", _HazBlock.size, _HazBlock.minColour.components[Colour::Red], _HazBlock.minColour.components[Colour::Blue], _HazBlock.minColour.components[Colour::Green], _HazBlock.minColour.components[Colour::Alpha]);
-	}
+        pc.printf(	"HazBlock:\n \t Size:%i\n \t Real Colour:%i,%i,%i,%i\n \tMin Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i\n ", _HazBlock.size, colourValue[0], colourValue[1], colourValue[2], colourValue[3], _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]);
+    } else if (typeRaw == Query) {
+        pc.printf(	"HazBlock:\n \t Size:%i\n \t Min Colour:%i,%i,%i,%i\n \t Max Colour:%i,%i,%i,%i\n ", _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]);
+    }
 }
 
-void getCurrentBlock(CommandTypeRaw typeRaw){
-	pc.printf("getting current block readings\n");
+void getCurrentBlock(CommandTypeRaw typeRaw)
+{
+    pc.printf("getting current block readings\n");
 }
 
-void setIntegrationTime(int integrationTime){
-	pc.printf("setting i-time to %i.\n", integrationTime);
+void setIntegrationTime(int integrationTime)
+{
+    rgbSensor.setIntegrationTime(integrationTime);
+    pc.printf("setting i-time to %i.\n", integrationTime);
 }
 
-void previewOnPC(bool on){
-	pc.printf("setting preview on pc to %i.\n", on);
+void previewOnPC(bool on)
+{
+    pc.printf("setting preview on pc to %i.\n", on);
 }
 
-void testServos(){
-	pc.printf("testing servos.\n");
+void readColourSensor(){
+	int colourValue[4];
+        rgbSensor.getAllColors(colourValue);
+        pc.printf(	"Colour Reading:%i,%i,%i,%i\n", colourValue[0], colourValue[1], colourValue[2], colourValue[3]);
+        
 }
 
-void resetServos(){
-	pc.printf("resetting servos.\n");
+
+void testServos()
+{
+    runServoTest = true;
 }
 
-void getPortInfo(){
-	pc.printf("getting port info.\n");
+void resetServos()
+{
+    pc.printf("resetting servos.\n");
 }
 
-void setPortBaudRate(int baudRate){
-	pc.printf("setting port baudrate to: %i\n", baudRate);
+void getPortInfo()
+{
+    pc.printf("getting port info.\n");
 }
 
+void setPortBaudRate(int baudRate)
+{
+    pc.printf("setting port baudrate to: %i\n", baudRate);
+}
+