/ - Diff - AMiRo-OS - Research for Cognitive Interaction

Revision 7520e117

     #include "amiro_map.hpp"
     #include <cstdint>
     // #include <cstdint>
     // #include <math.h>
     uint8_t AmiroMap::initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){
     uint8_t AmiroMap::initialize(){
       // Clear old values in case the map is initialized again
       uint8_t visitTable[MAX_NODES];
       this->current = 0;
       this->next = 0;
       this->valid = false;
       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(config[i][2] == 0xff && i != 0){
         if(global->testmap[i][2] == 0xff && i != 0){
           break;
         } else if (config[i][2] == 0xff && i == 0) {
           this->valid = false;
         } else if (global->testmap[i][2] == 0xff && i == 0) {
           this->state.valid = false;
           return 255;
+        }
         //look for start node (default is Node 0)
         if (config[i][2] == 1 ) {
           this->current= i;
         if (global->testmap[i][2] == 1 ) {
           this->state.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->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++;
+      }
-...
           if (this->nodeList[k].visited == 1) {
             this->nodeList[k].visited = 0;
           } else {
             this->valid = false;
             this->state.valid = false;
             return k;
+          }
+        }
+      }
       this->valid = true;
       this->state.valid = true;
       return 42;
+    }
-...
+      }
+    }
     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;
+      }
     void AmiroMap::update(bool left, bool right, LineFollowStrategy strategy) {
       // 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
       //   }
       // }
+    }

     #include "linefollow.hpp"
     #include <amiroosconf.h>
     #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;
       uint8_t visited;
       types::position pL; // Left
       types::position pR; // Right
       types::position pB; // Back
     };
       struct node {
         uint8_t id;
         uint8_t flag;
         uint8_t left;
         uint8_t right;
         uint8_t visited;
         types::position pL; // Left
         types::position pR; // Right
         types::position pB; // Back
       };
       struct map_state {
         // 0 - left, 1- right
         uint8_t strategy;
         // Node ID of last detected fixpoint
         uint8_t current;
         // Next node ID
         uint8_t next;
         //Traveled Distance between current and next in %
         uint8_t dist;
         // True if the current loaded map is valid
         bool valid;
       };
     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; }
       map_state * get_state() { return &state; }
       AmiroMap(Global *global) : global{global} {}
-...
        * @param config map configuration array
+       *
        */
      uint8_t initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]);
       uint8_t initialize();
       /**
          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
        * 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);
       void update(LineFollowStrategy strategy);
     private:
       Global *global;
       bool valid = false;
       LineFollowStrategy lfStrategy = LineFollowStrategy::EDGE_RIGHT;
       // Keeps track of the internal map state
       map_state state;
       // Holds the amount of detected node of the map
       uint8_t nodeCount = 0;
       // Stores all nodes and the corresponding coordinates of last detected fixpoints
       node nodeList[MAX_NODES];
       uint8_t current = 0;
       uint8_t next = 0;
       // If driving over fixpoint prevent continuous updating
       // True if left sensor is driving over black
       bool leftDetected = false;
       // True if right sensor is driving over black
       bool rightDetected = false;
       /**
        * Recursively search through all nodes in the node list and
        * mark all reached nodes as visited.
+       *
        * @param id node from where to start visiting the other nodes
+       *
        */
       void visitNode(uint8_t id);
       };
     }; // namespace amiro

Also available in: Unified diff