AMiRo-OS

  int IDLE                = 0;

  int FOLLOW_LINE         = 1;

  int DETECT_STATION      = 2;

  int REVERSE             = 3;

  int PUSH_BACK           = 4;

  int CHECK_POSITIONING   = 5;

  int CHECK_VOLTAGE       = 6;

  int CHARGING            = 7;

  int RELEASE             = 8;

  int RELEASE_TO_CORRECT  = 9;

  int CORRECT_POSITIONING = 10;

  int TURN                = 12;

  int INACTIVE            = 13;

  int CALIBRATION         = 14;

  int CALIBRATION_CHECK   = 15;

  int DEVIATION_CORRECTION = 16;

  int DOCKING_ERROR       = 16+1;

  int REVERSE_TIMEOUT_ERROR   = 16+2;

  int CALIBRATION_ERROR   = 16+3;

  int WHITE_DETECTION_ERROR   = 16+4;

  int PROXY_DETECTION_ERROR   = 16+5;

  int NO_CHARGING_POWER_ERROR   = 16+6;

  int 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;

  }

inline void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES], uint8_t id,

                   uint8_t l, uint8_t r, uint8_t att) {

  map[id][0] = l;

  map[id][1] = r;

  map[id][2] = att;

}

#define TEST_CASES 10

void shellRequestTestSuite(BaseSequentialStream *chp, int argc, char *argv[]){

  uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES] = {0};

  bool testres[TEST_CASES];

  setAttributes(testmap, 0, 1, 2, 1);

  setAttributes(testmap, 1, 2, 2, 0);

  setAttributes(testmap, 2, 1, 0, 0);

  setAttributes(testmap, 3, 0, 0, 0xff);

  AmiroMap map = AmiroMap(&global);

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

  int tcase = 0;

  map.initialize(testmap);

  testres[tcase] = map.get_valid();

  uint8_t ini = map.initialize(testmap);

  chprintf(chp, "Initialized returned: %d \n", ini);

  BaseThread::sleep(CAN::UPDATE_PERIOD);

  tcase++; // 1

  setAttributes(testmap, 0, 1, 2, 0xff);

  ini =  map.initialize(testmap);

  chprintf(chp, "Initialized returned: %d \n", ini);

  testres[tcase] = !map.get_valid();

  BaseThread::sleep(CAN::UPDATE_PERIOD);

  tcase++; // 2

  setAttributes(testmap, 0, 1, 2, 0);

  setAttributes(testmap, 2, 1, 0, 1);

  map.initialize(testmap);

  testres[tcase] = map.get_current() == 2;

  BaseThread::sleep(CAN::UPDATE_PERIOD);

  tcase++; // 3

  setAttributes(testmap, 3, 0, 0, 0);

  setAttributes(testmap, 4, 0, 0, 0xff);

  map.initialize(testmap);

  testres[tcase] = !map.get_valid();

  BaseThread::sleep(CAN::UPDATE_PERIOD);

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

  int failed = 0;

  int passed = 0;

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

    if (testres[i]){

      passed++;

      chprintf(chp, "Test %d Passed!\n", i);

    }else{

      failed++;

      chprintf(chp, "Test %d Failed\n", i);

    }

    BaseThread::sleep(CAN::UPDATE_PERIOD);

  }

  chprintf(chp, "Total: %d, Passed: %d, Failed: %d\n", tcase + 1, passed, failed);

}

  // chprintf(chp, "  +-------------------+\n");

  // chprintf(chp, "  |                   |\n");

  // chprintf(chp, "  |                   |\n");

  // chprintf(chp, "+-v--+                |\n");

  // chprintf(chp, "|  0 |              +-++\n");

  // chprintf(chp, "+-+--+     +--------+2 | <-------+\n");

  // chprintf(chp, "  |        |        +--+         |\n");

  // chprintf(chp, "  |        |        ^            |\n");

  // chprintf(chp, "  |        |        |            |\n");

  // chprintf(chp, "  |        v        |            |\n");

  // chprintf(chp, "  |      +-+-+------+            |\n");

  // chprintf(chp, "  +------> 1 |                   |\n");

  // chprintf(chp, "         +---+-------------------+\n");

  uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES] = {0};

  setAttributes(testmap, 0, 1, 2, 1);

  setAttributes(testmap, 1, 2, 2, 0);

  setAttributes(testmap, 2, 1, 0, 0);

  setAttributes(testmap, 3, 0, 0, 0xff);

  AmiroMap map = AmiroMap(&global);

  map.initialize(testmap);

}

    {"testMap", shellRequestMapTest},

    {"test", shellRequestTestSuite },