diff --git a/main.c b/main.c
index da26074..83f03a4 100644
--- a/main.c
+++ b/main.c
@@ -25,11 +25,16 @@ void setup (void) {			// All the blackbox code
 	gpio_set_mode(GPIOA, GPIO_MODE_INPUT, GPIO_CNF_INPUT_FLOAT, GPIO0);
 }
 
+void initial_wait (void) {
+	gpio_clear(GPIOC, GPIO8 | GPIO9);
+	while (gpio_get(GPIOA, GPIO0) == 0);
+	return;
+}
+
 int main (void) {
 	setup();
 
-	gpio_clear(GPIOC, GPIO8 | GPIO9);
-	while (gpio_get(GPIOA, GPIO0) == 0);
+	initial_wait();
 
 	// Array of what leds should be on
 	uint16_t states[4] = {0, GPIO9, GPIO8, GPIO9 | GPIO8};
@@ -37,27 +42,25 @@ int main (void) {
 	bool pressed = true;
 	bool change = false;
 	char state = 0;
-	int timer;
+	int i;
 
 	while (true) {
-		timer = DELAY;
 		change = false;
-		repeat:
-		if (/*Button pressed, but was not*/ gpio_get(GPIOA, GPIO0) && (pressed == false) ) {
-			pressed = true;
-			if (change == false) {
-				direction = (direction == FORWARD ? BACKWARD : FORWARD);	// Switch direction
+		for (i=0 ; i<DELAY ; i++) {
+			if (/*Button pressed, but was not*/ gpio_get(GPIOA, GPIO0) && (pressed == false) ) {
+				pressed = true;
 				change = true;
+			} else if (/*Button not pressed*/gpio_get(GPIOA, GPIO0) == 0)  {
+				pressed = false;
 			}
-		} else if (/*Button not pressed*/gpio_get(GPIOA, GPIO0) == 0)  {
-			pressed = false;
 		}
-		timer--;
-		if (timer > 0) goto repeat;
-		state = (direction == FORWARD ? (state + 1)%4 : (/*state+4-1*/ state+3)%4 );
+
+		if (change) direction = (direction == FORWARD ? BACKWARD : FORWARD);	// Switch direction
+		state = (direction == FORWARD ? (state + 1)%4 : (/*state+4-1*/ state+3)%4 ); // New state
 
 		gpio_set(GPIOC, states[state]);
 		gpio_clear(GPIOC, (GPIO8 | GPIO9) ^ states[state]);
 	}
 	return 0;
 }
+