AMiRo-OS

#include "amiro_map.hpp"

// #include <cstdint>

// #include <math.h>

uint8_t AmiroMap::initialize(){

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

  this->state.current = 0;

  this->state.next = 0;

  this->state.valid = false;

  // convert proto map to internal representation

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

    if(global->testmap[i][2] == 0xff && i != 0){

      break;

    } else if (global->testmap[i][2] == 0xff && i == 0) {

      this->state.valid = false;

      return 255;

    }

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

    if (global->testmap[i][2] == 1 ) {

      this->state.current = i;

    }

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

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

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

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

    this->nodeCount++;

  }

  // TODO make validity check

  for (int j=0; j<nodeCount; j++) {

    this->nodeList[j].visited = 0;

    visitNode(j);

    for (int k = 0; k < nodeCount; k++) {

      if (this->nodeList[k].visited == 1) {

        this->nodeList[k].visited = 0;

      } else {

        this->state.valid = false;

        return k;

      }

    }

  }

  this->state.valid = true;

  return 42;

}

void AmiroMap::visitNode(uint8_t id){

  if (this->nodeList[id].visited == 1){

    return;

  }else{

    nodeList[id].visited = 1;

    visitNode(this->nodeList[id].left);

    visitNode(this->nodeList[id].right);

  }

}

void AmiroMap::update(LineFollowStrategy strategy) {

  // Each time at the end of the user thread state maschine, when the strategy

  // is changed or a fixpoint is detected, this method gets called.

  // The strategy change only has an effect on the fixpoint or in particular if

  // a fixpoint is detected.

  // set the strategy directly, actually there is no need to store that variable in the class

  // but we will go with it for now to initialize everything properly.

  this->lfStrategy = strategy;

  uint16_t WL = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();

  uint16_t WR = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();

  // Check the wheel sensors

  bool left = global->linePID.BThresh >= WL;

  bool right = global->linePID.BThresh >= WR;

  types::position currentPos = global->odometry.getPosition();

  if (left && right) {

    // TODO A dangerous case -> amiro could be lifted

  }

  else if (left && !leftDetected) {

    // The sensor on the left side of the Amiro is driving on black

    // To prevent continous fixpoint detection a point needs to be marked as currently detected

    // and released.

    leftDetected = true;

    nodeList[state.next].pR.f_x = currentPos.f_x;

    nodeList[state.next].pR.f_y = currentPos.f_y;

    nodeList[state.next].pR.f_z = currentPos.f_z;

    nodeList[state.next].pR.x = currentPos.x;

    nodeList[state.next].pR.y = currentPos.y;

    nodeList[state.next].pR.z = currentPos.z;

    nodeList[state.next].visited |= 0x01;

    state.current = state.next;

    state.next = nodeList[state.current].right;

    state.strategy = 0x01;

  }

  else if (right && !rightDetected) {

    // Same as left only for the right sensor.

    rightDetected = true;

    nodeList[state.next].pL.f_x = currentPos.f_x;

    nodeList[state.next].pL.f_y = currentPos.f_y;

    nodeList[state.next].pL.f_z = currentPos.f_z;

    nodeList[state.next].pL.x = currentPos.x;

    nodeList[state.next].pL.y = currentPos.y;

    nodeList[state.next].pL.z = currentPos.z;

    nodeList[state.next].visited |= 0x02;

    state.current = state.next;

    state.next = nodeList[state.current].left;

    state.strategy = 0x02;

  }

  else if (!left && !right) {

    // in case the fixpoint is not detected anymore

    leftDetected = false;

    rightDetected = false;

  }

  // update internal map_state

  // Update travel distance

  // check if the nodes of the specific strategy where visited

  // if (state.strategy

  //     == (nodeList[state.current].visited & nodeList[state.next].visited)) {

  //   // only update distance if both nodes were visited

  //   if (state.strategy == 0x01) {

  //     // Amiro is driving on the right edge

  //     state.dist = (uint8_t) sqrt(

  //                                 pow(nodeList[state.next].pR.x - nodeList[state.current].pR.x, 2)

  //                                 + pow(nodeList[state.next].pR.y - nodeList[state.current].pR.y, 2));

  //   } else {

  //     // Driving on the left edge

  //   }

  // }

}