AMiRo-OS

  uint32_t stateTracker[24] = { 0 };

  uint32_t stateTransitionCounter = 0;

  uint8_t IDLE                = 0;

  uint8_t FOLLOW_LINE         = 1;

  uint8_t DETECT_STATION      = 2;

  uint8_t REVERSE             = 3;

  uint8_t PUSH_BACK           = 4;

  uint8_t CHECK_POSITIONING   = 5;

  uint8_t CHECK_VOLTAGE       = 6;

  uint8_t CHARGING            = 7;

  uint8_t RELEASE             = 8;

  uint8_t RELEASE_TO_CORRECT  = 9;

  uint8_t CORRECT_POSITIONING = 10;

  uint8_t TURN                = 12;

  uint8_t INACTIVE            = 13;

  uint8_t CALIBRATION         = 14;

  uint8_t CALIBRATION_CHECK   = 15;

  uint8_t DEVIATION_CORRECTION = 16;

  uint8_t DOCKING_ERROR       = 16+1;

  uint8_t REVERSE_TIMEOUT_ERROR   = 16+2;

  uint8_t CALIBRATION_ERROR   = 16+3;

  uint8_t WHITE_DETECTION_ERROR   = 16+4;

  uint8_t PROXY_DETECTION_ERROR   = 16+5;

  uint8_t NO_CHARGING_POWER_ERROR   = 16+6;

  uint8_t UNKNOWN_STATE_ERROR   = 16+7;

  chprintf(chp, "IDLE: %d\n", global.stateTracker[IDLE]);

  chprintf(chp, "FOLLOW_LINE: %d\n", global.stateTracker[FOLLOW_LINE]);

  chprintf(chp, "DETECT_STATION: %d\n", global.stateTracker[DETECT_STATION]);

  chprintf(chp, "REVERSE: %d\n", global.stateTracker[REVERSE]);

  chprintf(chp, "PUSH_BACK: %d\n", global.stateTracker[PUSH_BACK]);

  chprintf(chp, "CHECK_POSITIONING: %d\n", global.stateTracker[CHECK_POSITIONING]);

  chprintf(chp, "CHECK_VOLTAGE: %d\n", global.stateTracker[CHECK_VOLTAGE]);

  chprintf(chp, "CHARGING: %d\n", global.stateTracker[CHARGING]);

  chprintf(chp, "RELEASE: %d\n", global.stateTracker[RELEASE]);

  chprintf(chp, "RELEASE_TO_CORRECT: %d\n", global.stateTracker[RELEASE_TO_CORRECT]);

  chprintf(chp, "CORRECT_POSITIONING: %d\n", global.stateTracker[CORRECT_POSITIONING]);

  chprintf(chp, "TURN: %d\n", global.stateTracker[TURN]);

  chprintf(chp, "INACTIVE: %d\n", global.stateTracker[INACTIVE]);

  chprintf(chp, "CALIBRATION: %d\n", global.stateTracker[CALIBRATION]);

  chprintf(chp, "CALIBRATION_CHECK: %d\n", global.stateTracker[CALIBRATION_CHECK]);

  chprintf(chp, "DEVIATION_CORRECTION: %d\n", global.stateTracker[DEVIATION_CORRECTION]);

  chprintf(chp, "DOCKING_ERROR: %d\n", global.stateTracker[DOCKING_ERROR]);

  chprintf(chp, "REVERSE_TIMEOUT_ERROR: %d\n", global.stateTracker[REVERSE_TIMEOUT_ERROR]);

  chprintf(chp, "CALIBRATION_ERROR: %d\n", global.stateTracker[CALIBRATION_ERROR]);

  chprintf(chp, "WHITE_DETECTION_ERROR: %d\n", global.stateTracker[WHITE_DETECTION_ERROR]);

  chprintf(chp, "PROXY_DETECTION_ERROR: %d\n", global.stateTracker[PROXY_DETECTION_ERROR]);

  chprintf(chp, "NO_CHARGING_POWER_ERROR: %d\n", global.stateTracker[NO_CHARGING_POWER_ERROR]);

  chprintf(chp, "UNKNOWN_STATE_ERROR: %d\n", global.stateTracker[UNKNOWN_STATE_ERROR]);

  for (int i=0; i<24;i++){

    global.stateTracker[i] = 0;

  }

      // Test suit for amiro map

      // --------------------------------------------------

    // In case a new state is set:

    // 1. Record the state transition

      global.stateTransitionCounter++;

      // Clear all state transitions to prevent overflow

      if (global.stateTransitionCounter >= 255) {

        global.stateTransitionCounter = 0;

        for (int i = 0; i < 24; i++) {

          global.stateTracker[i] = 0;

        }

}

      // Transmit the new state over can

      // Increase state count for specific state

      // TODO: Improve with dictionary or other than switch case

      if (newState == states::IDLE)

        {global.stateTracker[states::IDLE] += 1;}

      else if (newState == states::FOLLOW_LINE)

        {global.stateTracker[states::FOLLOW_LINE] += 1;}

      else if (newState == states::DETECT_STATION)

        {global.stateTracker[states::DETECT_STATION] += 1;}

      else if (newState == states::REVERSE)

        {global.stateTracker[states::REVERSE] += 1;}

      else if (newState == states::PUSH_BACK)

        {global.stateTracker[states::PUSH_BACK] += 1;}

      else if (newState == states::CHECK_POSITIONING)

        {global.stateTracker[states::CHECK_POSITIONING] += 1;}

      else if (newState == states::CHECK_VOLTAGE)

        {global.stateTracker[states::CHECK_VOLTAGE] += 1;}

      else if (newState == states::CHARGING)

        {global.stateTracker[states::CHARGING] += 1;}

      else if (newState == states::RELEASE)

        {global.stateTracker[states::RELEASE] += 1;}

      else if (newState == states::RELEASE_TO_CORRECT)

        {global.stateTracker[states::RELEASE_TO_CORRECT] += 1;}

      else if (newState == states::CORRECT_POSITIONING)

        {global.stateTracker[states::CORRECT_POSITIONING] += 1;}

      else if (newState == states::TURN)

        {global.stateTracker[states::TURN] += 1;}

      else if (newState == states::INACTIVE)

        {global.stateTracker[states::INACTIVE] += 1;}

      else if (newState == states::CALIBRATION)

        {global.stateTracker[states::CALIBRATION] += 1;}

      else if (newState == states::CALIBRATION_CHECK)

        {global.stateTracker[states::CALIBRATION_CHECK] += 1;}

      else if (newState == states::DEVIATION_CORRECTION)

        {global.stateTracker[states::DEVIATION_CORRECTION] += 1;}

      else if (newState == states::DOCKING_ERROR)

        {global.stateTracker[16+(-states::DOCKING_ERROR)] += 1;}

      else if (newState == states::REVERSE_TIMEOUT_ERROR)

        {global.stateTracker[16+(-states::REVERSE_TIMEOUT_ERROR)] += 1;}

      else if (newState == states::CALIBRATION_ERROR)

        {global.stateTracker[16+(-states::CALIBRATION_ERROR)] += 1;}

      else if (newState == states::WHITE_DETECTION_ERROR)

        {global.stateTracker[16+(-states::WHITE_DETECTION_ERROR)] += 1;}

      else if (newState == states::PROXY_DETECTION_ERROR)

        {global.stateTracker[16+(-states::PROXY_DETECTION_ERROR)] += 1;}

      else if (newState == states::NO_CHARGING_POWER_ERROR)

        {global.stateTracker[16+(-states::NO_CHARGING_POWER_ERROR)] += 1;}

      else if (newState == states::UNKNOWN_STATE_ERROR)

        {global.stateTracker[16+(-states::UNKNOWN_STATE_ERROR)] += 1;}

    // Keep track of last state and set the new state for next iteration