/devices/DiWheelDrive/amiro_map.cpp - Diff - AMiRo-OS - Research for Cognitive Interaction

Revision 64cba697 devices/DiWheelDrive/amiro_map.cpp

+        }
         this->nodeList[i].id = i;
         this->nodeList[i].left = global->testmap[i][0];
         this->nodeList[i].right = global->testmap[i][1];
         this->nodeList[i].edge.edge_id.left = global->testmap[i][0];
         this->nodeList[i].edge.edge_id.right = global->testmap[i][1];
         this->nodeList[i].flag = global->testmap[i][2];
         this->nodeCount++;
+      }
       this->state.next = this->nodeList[this->state.current].right;
       this->state.next = this->nodeList[this->state.current].edge.edge_id.right;
       // TODO make validity check
-...
         return;
       }else{
         nodeList[id].visited = 1;
         visitNode(this->nodeList[id].left);
         visitNode(this->nodeList[id].right);
         visitNode(this->nodeList[id].edge.edge_id.left);
         visitNode(this->nodeList[id].edge.edge_id.right);
+      }
+    }
-...
       // 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;
-...
         // TODO A dangerous case -> amiro could be lifted
         flag |= 255;
+      }
       else if (left && !leftDetected) {
       else if (left && !fxpDetected) { // Driving on the right edge
         // 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;
         state.strategy = 0x02;
         switchToNext(&currentPos);
         flag |= 0x2;
+      }
       else if (right && !rightDetected) {
       else if (right && !fxpDetected) { // Driving on the left edge
         // 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;
         state.strategy = 0x01;
         switchToNext(&currentPos);
         flag |= 0x1;
+      }
       else if (!left && !right) {
         // in case the fixpoint is not detected anymore
         leftDetected = false;
         rightDetected = false;
         fxpDetected = false;
         flag |= 0x4;
+      }
       // Update dist and edge length if possible
       calTravelState(&currentPos);
       // 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;
+    }
-...
       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)) {
       // Do not update if update was already applied the round before (|| rightDetected) == true
       if ((left || right) && !fxpDetected) {
         // Determine what strategy to use
         // assignFxp() will use strategy to assign the next point
         state.strategy = right ? 1 : 2;
         state.strategy = left ? 1 : 2;
         // Check if next point is reachable
         if (state.next == 255){
-...
       }else if (!(left || right)) {
         // TODO: do we need both?
         leftDetected = rightDetected = true;
         fxpDetected = 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 =
       // if current was visited
       if ((state.strategy & nodeList[state.current].visited) > 0) {
         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) > 0)) {
           state.eLength =
               calculateDist(&nodeList[state.current].p.arr[state.strategy - 1],
                             &nodeList[state.next].p.arr[state.strategy - 1]);
+        }
+      }
     void AmiroMap::checkMap() {
       // The check will basically only consist in checking if all nodes
-...
           nodeList[state.next].visited |= state.strategy;
+      }
       leftDetected = true;
       fxpDetected = 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

Also available in: Unified diff