Diff: globals.cpp

Revision:

18:44a1c1a30166

Parent:

17:af373246bf80

Child:

19:61b21ac4896e

--- a/globals.cpp	Mon Nov 23 23:13:39 2015 +0000
+++ b/globals.cpp	Tue Nov 24 00:24:39 2015 +0000
@@ -15,112 +15,116 @@
 bool connectedToPC = false;
 bool runServoTest = false;
 PCModes currentMode = None;
+Controls currentState = Pause;
 
 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;
-    }
+	if(i2cport->read_bit(8)) {
+		while (i2cport->read_bit(8)) { }
+		return 1;
+	} else if (i2cport->read_bit(9)) {
+		while (i2cport->read_bit(9)) { }
+		return 2;
+	} else if (i2cport->read_bit(10)) {
+		while (i2cport->read_bit(10)) { }
+		return 3;
+	} else if (i2cport->read_bit(11)) {
+		while (i2cport->read_bit(11)) { }
+		return 4;
+	} else {
+		return 0;
+	}
 
 }
 
 void connectToPC(CommandTypeRaw typeRaw)
 {
-    connectedToPC = true;
-    pc.printf("INFO:MBED connected to PC.\n");
+	connectedToPC = true;
+	pc.printf("INFO:MBED connected to PC.\n");
 }
 
 void disconnectToPC(CommandTypeRaw typeRaw)
 {
-    pc.printf("INFO:MBED disconnected from PC.\n");
-    connectedToPC = false;
-    currentMode = None;
+	pc.printf("INFO:MBED disconnected from PC.\n");
+	connectedToPC = false;
+	currentMode = None;
 }
 
 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();
+	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);
 
-        _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(Stop);
-        fpga->moveSortingServo(NonHaz);
-		
-        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]);
-    }
+		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(Stop);
+		fpga->moveSortingServo(NonHaz);
+
+		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("DEBUG: Getting current block readings\n");
+	pc.printf("DEBUG: Getting current block readings\n");
 }
 
 void setIntegrationTimeTo(float integrationTime)
 {
-    rgbSensor.setIntegrationTime(integrationTime);
-    pc.printf("DEBUG: Setting integration-time to %i.\n", integrationTime);
+	rgbSensor.setIntegrationTime(integrationTime);
+	pc.printf("DEBUG: Setting integration-time to %i.\n", integrationTime);
 }
 
 void previewOnPC(bool on)
 {
-    pc.printf("setting preview on pc to %i.\n", on);
+	pc.printf("setting preview on pc to %i.\n", on);
 }
 
 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]);
-        
+	rgbSensor.getAllColors(colourValue);
+	pc.printf(	"Colour Reading:%i,%i,%i,%i\n", colourValue[0], colourValue[1], colourValue[2], colourValue[3]);
+
 }
 
 
 void testServos(Controls state)
 {
-    if (state == Start){
-    pc.printf("INFO: Running servo test.\n");
-    runServoTest = true;
+	if (state == Start){
+		pc.printf("INFO: Running servo test.\n");
+		runServoTest = true;
 	}else if (state == Pause){
 		pc.printf("INFO: Finished running servo test.\n");
 		runServoTest = false;
@@ -129,19 +133,19 @@
 
 void resetServos()
 {
-    pc.printf("resetting servos.\n");
+	pc.printf("resetting servos.\n");
 }
 
 void getPortInfo()
 {
-    pc.printf("getting port info.\n");
+	pc.printf("getting port info.\n");
 }
 
 void setPortBaudRate(int baudRate)
 {
 	pc.baud(baudRate);
 	wait(0.1);
-    pc.printf("DEBUG: Setting port Baud Rate to: %i.\n", baudRate);
+	pc.printf("DEBUG: Setting port Baud Rate to: %i.\n", baudRate);
 }
 
 void setPortParity(int parity){