AMiRo-OS

void AmiroMap::initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){

  // Clear old values in case the map is initialized again

  this->current = 0;

  this->next = 0;

  this->valid = false;

  // convert proto map to internal representation

    if(config[i][2] == 0xff && i != 0){

      this->valid = true;

    } else if (config[i][2] == 0xff && i == 0) {

      this->valid = false;

      return;

    //look for start node (default is Node 0)

    if (config[i][2] == 1 ) {

      this->current= i;

    }

    this->nodeList[i].id = i;

    this->nodeList[i].left = config[i][0];

    this->nodeList[i].right = config[i][1];

    this->nodeList[i].flag = config[i][2];

    this->nodeCount++;

  }

  // TODO make validity check

void AmiroMap::update(bool left, bool right, LineFollowStrategy strategy) {

#include "linefollow.hpp"

#include <ch.hpp>

#include <cstdint>

#define MAX_NODES 10

#define NODE_ATTRIBUTES 3

namespace amiro {

struct node {

  uint8_t id;

  uint8_t flag;

  uint8_t left;

  uint8_t right;

  types::position pL; // Left

  types::position pR; // Right

  types::position pB; // Back

};

class AmiroMap {

public:

  uint8_t get_next()  { return next; }

  void set_next( uint8_t next) { this->next = next; }

  uint8_t get_current()  { return current; }

  void set_current( uint8_t current) { this->current = current; }

  node* get_nodeList()  { return nodeList; }

  uint8_t get_nodeCount()  { return nodeCount; }

  void set_nodeCount( uint8_t nodeCount) { this->nodeCount = nodeCount; }

  bool get_valid()  { return valid; }

  void set_valid( bool valid) { this->valid = valid; }

  AmiroMap(Global *global) : global{global} {}

  /**

   * Initialize a new map from configuration.

   * @param config map configuration array

   *

   */

  void initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]);

  /**

     Update the internal map state according to the detected fixpoint

     This function should be called for a generic update, each can cycle and in

     case a fixpoint on one side is detected.

     Internal state maschine will go into an error state in case both sensors are black.

     @param left fixpoint on left side detected

     @param right fixpoint on right side detected

     @param strategy current line follow strategy

   */

  void update(bool left, bool right, LineFollowStrategy strategy);

private:

  Global *global;

  bool valid = false;

  uint8_t nodeCount = 0;

  node nodeList[MAX_NODES];

  uint8_t current = 0;

  uint8_t next = 0;

};

}; // namespace amiro

#include <cstdint>

#include "linefollow.hpp"

#include "amiro_map.hpp"

 *

 *

    chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n",

    chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,Delta:%d\n",

  int32_t res = 0;

    // sign =

    // sign =

}

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();

  tcase++; // 1

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

  map.initialize(testmap);

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

  tcase++; // 2

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

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

  map.initialize(testmap);

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

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

  int failed = 0;

  int passed = 0;

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

    if (testres[i]){

      passed++;

    }else{

      failed++;

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

    }

  }

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

}

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

  // TODO:

  // For now see everything fixed. Create array with pointer to all node structs. Determine

  // 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);

    {"shutdown", shellRequestShutdown},

    {"wakeup", shellRequestWakeup},

    {"check", shellRequestCheck},

    {"reset_memory", shellRequestResetMemory},

    {"get_board_id", shellRequestGetBoardId},

    {"set_board_id", shellRequestSetBoardId},

    {"get_memory_data", shellRequestGetMemoryData},

    {"get_vcnl", shellRequestGetVcnl},

    {"calib_vcnl_offset", shellRequestCalib},

    {"set_vcnl_offset", shellRequestSetVcnlOffset},

    {"reset_vcnl_offset", shellRequestResetVcnlOffset},

    {"get_vcnl_offset", shellRequestGetVcnlOffset},

    {"reset_Ed_Eb", shellRequestResetCalibrationConstants},

    {"get_Ed_Eb", shellRequestGetCalibrationConstants},

    {"set_Ed_Eb", shellRequestSetCalibrationConstants},

    {"get_robot_id", shellRequestGetRobotId},

    {"get_system_load", shellRequestGetSystemLoad},

    {"set_lights", shellRequestSetLights},

    {"shell_board", shellSwitchBoardCmd},

    {"get_bootloader_info", shellRequestGetBootloaderInfo},

    {"motor_drive", shellRequestMotorDrive},

    {"motor_stop", shellRequestMotorStop},

    {"motor_calibrate", shellRequestMotorCalibrate},

    {"motor_getGains", shellRequestMotorGetGains},

    {"motor_resetGains", shellRequestMotorResetGains},

    {"calibrate_line_sensors", shellRequestCalibrateLineSensores},

    {"printProxyBottom", sellRequestgetBottomSensorData},

    {"printProxyRing", shellRequestProxyRingValues},

    {"printMagnetometer", shellRequestMagnetoMeter},

    {"printMagnetometerRes", shellRequestMagnetoMeterPrint},

    {"printLocation", shellRequestPrintCoordinate},

    {"checkPowerPins", shellRequestCheckPower},

    {"stateInfos", shellRequestErrorInfo},

    {"testMap", shellRequestMapTest},

    {"test", shellRequestTestSuite },

    {NULL, NULL}};