AMiRo-OS

#include "amiro_map.hpp"

#include "linefollow.hpp"

#include <cstdint>

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;

  this->nodeCount = 0;

  this->state.strategy = 0x1;

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

  }

  this->state.next = this->nodeList[this->state.current].right;

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

  }

}

uint8_t AmiroMap::update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy) {

  // Called each time at the end of the user thread state machine

  // The bottom sensors will be checked for black ground which is interpreted as

  // filxpoint

  // 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.

  uint8_t flag = 0;

  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

    flag |= 255;

  }

  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;

    copyPoint(&currentPos, &nodeList[state.next].pR);

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

    state.current = state.next;

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

    state.strategy = 0x01;

    state.eLength = 0; // Reset length to get recalculated after fixpoint

    flag |= 0x1;

  }

  else if (right && !rightDetected) {

    // Same as left only for the right sensor.

    rightDetected = true;

    copyPoint(&currentPos, &nodeList[state.next].pR);

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

    state.current = state.next;

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

    state.strategy = 0x2;

    state.eLength = 0; // Reset length to get recalculated after fixpoint

    flag |= 0x2;

  }

  else if (!left && !right) {

    // in case the fixpoint is not detected anymore

    leftDetected = false;

    rightDetected = false;

    flag |= 0x4;

  }

  // update internal map_state

  // Update travel distance

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

  if (state.strategy

      == nodeList[state.current].visited) {

    flag |= 0x8;

    // only update distance if both nodes were visited

    // Calculate estimated length of the edge

    if (state.strategy == 0x01) {

      // Amiro is driving on the right edge

      // only calculate edge length if the node is already vivited

      if ((state.eLength == 0) && (state.strategy == nodeList[state.current].visited)) {

        state.eLength = calculateDist(&nodeList[state.next].pR,

                                      &nodeList[state.current].pR);

      }

      state.dist = calculateDist(&nodeList[state.current].pR, &currentPos);

    } else {

      // Driving on the left edge

      if ((state.eLength == 0) &&

          (state.strategy == nodeList[state.current].visited)) {

        state.eLength = calculateDist(&nodeList[state.next].pR,

                                      &nodeList[state.current].pR);

      }

      state.dist = calculateDist(&nodeList[state.current].pL, &currentPos);

    }

  }

  return flag;

}

uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) {

  return (uint32_t)  sqrt(pow((p2->x - p1->x)/10000, 2) +

                          pow((p2->y - p1->y)/10000, 2));

}

uint8_t AmiroMap::trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy,

                    ut_states ut_state) {

  // Check if map is valid

  if (this->state.valid){

    return update(WL, WR, strategy);

  }

  // Create init node if none is there

  // We will not assign a point to the initial fixpoint because it is not clear if

  // start position is at the correct point

  if (nodeCount == 0) {

    createInitNode();

  }

  this->lfStrategy = strategy;

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

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

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

  // Assign fixpoint if side sensor is black

  // Do not update if update was already applied the round before (leftDetected || rightDetected) == true

  if ((left || right) && !(leftDetected || rightDetected)) {

    // Determine what strategy to use

    // assignFxp() will use strategy to assign the next point

    state.strategy = right ? 1 : 2;

    // Check if next point is reachable

    if (state.next == 255){

      // Prepare state values for switch

      assignFxp(&currentPos);

    }

  }else if (!(left || right)) {

    // TODO: do we need both?

    leftDetected = rightDetected = true;

  }

}

void AmiroMap::calTravelState(types::position *p1) {

  // Calculate the moved distance from last detected  fixpoint

  state.dist = calculateDist(p1, &nodeList[state.current].p.arr[state.strategy - 1]);

  // Calculate elength if it is 0

  // and if the point of the next node was visited before

  if ((state.eLength == 0) && ((state.strategy & nodeList[state.next].visited) == 1)) {

    state.eLength =

        calculateDist(p1, &nodeList[state.current].p.arr[state.strategy - 1]);

  }

}

void AmiroMap::checkMap() {

  // The check will basically only consist in checking if all nodes

  // are connected to following nodes

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

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

      if(nodeList[i].edge.arr[j] == 255){

        state.valid = false;

        return;

      }

  }

  state.valid = true;

}

void AmiroMap::switchToNext(types::position *p1) {

  // Update point if node was not visited before

  if ((nodeList[state.next].visited & state.strategy) == 0){

      copyPoint(p1, &nodeList[state.next].p.arr[state.strategy - 1]);

      nodeList[state.next].visited |= state.strategy;

  }

  leftDetected = true;

  state.current = state.next;

  state.next = nodeList[state.current].edge.arr[state.strategy - 1];

  state.eLength = 0; // Reset length to get recalculated after fixpoint

  return;

}

void AmiroMap::copyPoint(types::position* from, types::position* to) {

  to->x = from->x;

  to->y = from->y;

  to->f_x = from->f_x;

}

void AmiroMap::createInitNode() {

  this->nodeCount = 0;

  state.current = addNode(255, 255, 1);

  state.next = 255;

}

uint8_t AmiroMap::getNearest(types::position *p1) {

  uint8_t actualStrategy = this->lfStrategy == EDGE_LEFT ? 1 : 2;

  uint32_t thresh = global->nodeDistThresh;          // TODO: find good thresh value in cm

  uint8_t id = 255;

  uint32_t smallestDist = thresh;

  uint8_t currentStrategy;

  // Calculate the point which is nearest to the current one

  // check if distance and strategy match

  // If right point is found but no left point set choose this as the fitting point

  // Check how point was visited before calculating distance (non visited points are always (0,0))

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

    for (int j = 0; j < 2; j++){ // Iterate over l and r point

      if ((nodeList[i].visited & (j+1)) == 0){

        // Skip point if it was not visited for the given strategy

        continue;

      }

      uint32_t tmpDist = calculateDist(&nodeList[i].p.arr[j], p1);

      if (tmpDist < smallestDist){

        smallestDist = tmpDist;

        id = i;

        // Store strategy to match the correct point at the end

        currentStrategy = j;

      }

    }

  }

  if (id == 255){

    return 255;

  }

  // update point at fixpoint if it is not visited

  if ((nodeList[id].visited & actualStrategy) == 0){

    copyPoint(p1, &nodeList[id].p.arr[actualStrategy]);

    nodeList[id].visited |= actualStrategy;

  } // else point was already visited and is assigned

  return id;

}

uint8_t AmiroMap::assignFxp(types::position *p1) {

  // Magic happens to determine if fixpoint is close enough

  uint8_t id = getNearest(p1);

  if(id < 255){

    // strategy is either 1 - right or 2 - left

    // Copy current point to either left or right point

    copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);

    }else {

    // A new fixpoint needs to be created

    id = addNode(255, 255, 0);

    copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);

  }

  // Prepare values for switching

  state.next = id;

  nodeList[state.current].edge.arr[state.strategy - 1] = id;

  // Mark point as visited

  nodeList[id].visited |= state.strategy;

  return id;

}

void AmiroMap::reset(){

  this->nodeCount = 0;

  this->state.current = 0;

  this->state.next= 0;

}