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

Revision 64cba697

+        }
         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

       node nodeList[MAX_NODES];
       // 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;
       bool fxpDetected = false;
       /**
        * Recursively search through all nodes in the node list and

           // No case should be true because neither was a node visited nor
           // was a fixpoint detected.
           // global.odometry
           global.testres[global.tcase] = (ret == 0x1)
             && (map.get_state()->strategy == 0x01)
           global.testres[global.tcase] = (ret == 0x2)
             && (map.get_state()->strategy == 0x02)
             && (map.get_state()->dist == 0)
             && (map.get_state()->current == 2);
-...
           // No case should be true because neither was a node visited nor
           // was a fixpoint detected.
           global.testres[global.tcase] = (ret == 0x00)
             && (map.get_state()->strategy == 0x01);
             && (map.get_state()->strategy == 0x02);
             // && (map.get_state()->dist == 0);
           global.odometry.setPosition(1.2, 0.0, 0.0);
-...
           // No case should be true because neither was a node visited nor
           // was a fixpoint detected.
           global.testres[global.tcase] =
               (ret == 0x04) && (map.get_state()->strategy == 0x01)
             && (map.get_state()->dist == 0);
               (ret == 0x04) && (map.get_state()->strategy == 0x02);
           global.odometry.setPosition(.5, 0.0, 0.0);
           chprintf((BaseSequentialStream *)&global.sercanmux1,
-...
           // No case should be true because neither was a node visited nor
           // was a fixpoint detected.
           global.testres[global.tcase] =
             (ret == 9) &&
             (map.get_state()->strategy == 1) &&
             (map.get_state()->dist == 0) &&
             (ret == 2) &&
             (map.get_state()->strategy == 2) &&
             // (map.get_state()->dist == 0) &&
             (map.get_state()->eLength == 50);
           global.odometry.setPosition(.75, 0.0, 0.0);
-...
           // No case should be true because neither was a node visited nor
           // was a fixpoint detected.
           global.testres[global.tcase] =
               (ret == 12) && (map.get_state()->strategy == 1) &&
               (map.get_state()->dist == 50) && (map.get_state()->eLength == 50);
               (ret == 4) && (map.get_state()->strategy == 2) &&
               // (map.get_state()->dist == 50)
              (map.get_state()->eLength == 50);
           int failed = 0;
           int passed = 0;

     // Map Tracking Parameters
     // enable amiro map to continuously build internal map representation
     #define AMIRO_MAP_AUTO_TRACKING true
     #define AMIRO_MAP_AUTO_TRACKING false
-...
       // Messages which can be received and trigger state changes
       public:
         // States of user thread state machine
         // enum ut_states : int8_t {
         //   UT_IDLE = 0,
         //   UT_FOLLOW_LINE = 1,
         //   UT_DETECT_STATION = 2,
         //   UT_REVERSE = 3,
         //   UT_PUSH_BACK = 4,
         //   UT_CHECK_POSITIONING = 5,
         //   UT_CHECK_VOLTAGE = 6,
         //   UT_CHARGING = 7,
         //   UT_RELEASE = 8,
         //   UT_RELEASE_TO_CORRECT = 9,
         //   UT_CORRECT_POSITIONING = 10,
         //   UT_TURN = 12,
         //   UT_INACTIVE = 13,
         //   UT_CALIBRATION = 14,
         //   UT_CALIBRATION_CHECK = 15,
         //   UT_DEVIATION_CORRECTION = 16,
         //   UT_TEST_MAP_STATE = 17,
         //   UT_TEST_MAP_AUTO_STATE = 18,
         //   UT_DOCKING_ERROR = -1,
         //   UT_REVERSE_TIMEOUT_ERROR = -2,
         //   UT_CALIBRATION_ERROR = -3,
         //   UT_WHITE_DETECTION_ERROR = -4,
         //   UT_PROXY_DETECTION_ERROR = -5,
         //   UT_NO_CHARGING_POWER_ERROR = -6,
         //   UT_UNKNOWN_STATE_ERROR = -7
         // };
         struct ut_counter {
           int whiteCount = 0;
           int ringProxCount = 0;
           // int correctionCount = 0;
           int errorCount = 0;
           int stateCount = 0;
           int stepCount = 0;
           uint32_t stateTime = 0;
       struct ut_counter {
         int whiteCount = 0;
         int ringProxCount = 0;
         // int correctionCount = 0;
         int errorCount = 0;
         int stateCount = 0;
         int stepCount = 0;
         uint32_t stateTime = 0;
       };
       struct proxy_ctrl {

Also available in: Unified diff