     }
 }
 
-uint8_t dispMode = 0;
+typedef enum disp_mode_t
+{
+  BATTERY, POWER, ANALOG, LED_TEST, NUM_MODES
+} DISP_MODE;
+
+#define SECONDS     10        // counts per second
+
+DISP_MODE dispMode = BATTERY;
 
 void loop()
 {

    Serial.begin(57600);
    pinMode(MODE_PIN, INPUT_PULLUP);

    analogReference(DEFAULT);
    led7seg.begin(COMMON_CATHODE, LED_DIGITS, ledPins);
	  pinMode(LED_BUILTIN, OUTPUT);
    pinMode(A6, INPUT);
    pinMode(A7, INPUT);
    pinMode(2, INPUT);
    pinMode(3, INPUT);
    digitalWrite(2, HIGH);    // Turn on pullup for D2
    digitalWrite(3, HIGH);    // Turn on pullup for D3



	if (millis() >= ten_msec) {
		ten_msec += 100;
		++counter;
   
    static int8_t oldMode = 1;
    int8_t mode_pin = digitalRead(MODE_PIN) ? 1 : 0;
    // Detect falling edge on MODE_PIN
    if ((oldMode ^ mode_pin) && !mode_pin)
    {
       dispMode = (DISP_MODE) ((dispMode + 1) % NUM_MODES);
       counter = 0;
    }
    oldMode = mode_pin;

    //               _______
    // FullRev  ____}       |_________________________|
    //                _______________                  
    // Stop      ____|               |________________|
    //                 _______________________         
    // FullFwd    ____|                       |_______|
    //
    int16_t power = encoder >> 2;
    int16_t usec = power * 5 + 1500;
    pwmOut.writeMicroseconds(usec);

    switch (dispMode) {
    case BATTERY:
      // Show battery voltage as xx.y
      battery = map(analogRead(A6), 0, 1023, 0, MAX_BATTERY);
      led7seg.showDecimal(battery, 1);
      // After 2 seconds if power != 0.00 switch modes
      if (counter >= 2*SECONDS && power != 0) {
          dispMode = POWER;
          counter = 0;
      }
      break;
    case POWER:
      // Display encoder value as signed -x.yy
      led7seg.showDecimal(power, 2);
      break;
    case ANALOG:
      // Display analog input as decimal
      analogin = analogRead(A7);
      led7seg.showDecimal(analogin, 0);
      break;
    default:
      LED_test();
      if (counter >= 3*SECONDS) {
        dispMode = BATTERY;
        counter = 0;
      }
    }
	}
	led7seg.refreshSegments(); // Refresh/multiplex display
