Rework main()

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);
+	initial_wait();
-	while (gpio_get(GPIOA, GPIO0) == 0);
 
 	// 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:
+		for (i=0 ; i<DELAY ; i++) {
 			if (/*Button pressed, but was not*/ gpio_get(GPIOA, GPIO0) && (pressed == false) ) {
 				pressed = true;
-			if (change == false) {
-				direction = (direction == FORWARD ? BACKWARD : FORWARD);	// Switch direction
 				change = true;
-			}
 			} 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;
 }
+