Embedded Motion Control 2015 Group 3/Decision

From Control Systems Technology Group
Jump to navigation Jump to search

Decision

The Decision block was made to be the highest in hierarchical order. It is called every iteration from the main. First, it calls the update function in the Scan block. One of the things this function does is recognizing the type of junction it is headed towards, when within range. It saves this junction in it's scandata, which is the class used for communication between blocks. This junction is an enum with several options:

  • Crossroad, where it can go left, right or forward.
  • Tjunction_1, where it can go left and right.
  • Tjunction_2, where it can go right and forward.
  • Tjunction_3, where it can go left and forward.
  • Deadend, where it is at a dead end, within the range required for opening the door.
  • Corridor, where it can see none of the above junction types, which means either the next junction is outside the range of detection, or the corridor it is following leads to a corner, which, since can be solved with the regular potential field driving, does not require a decision.


Next, the Decision block calls it's algorithm function. This function sends the junction type to the Mapping block, which handles it and returns the direction the robot should take next. The algorithm then stores this direction in the Scandata.


After the algorithm function finishes, the Decision part will first check if it is in a dead end. If it is, it will call to the Drive function which handles dead ends. Otherwise, it will check if it is following a corridor, then driving with the regular potential field, by first calling Scan to make the vector, then communicating this to Drive which follows it. Finally, if it is approaching an intersection and it has a direction, it will call a function constructing_walls within the Scan block. It will then call Scan to make the vector again, with virtual walls, and Drive to follow it. For each of these cases, it will also check if collision avoidance is needed, and calls this if necessary.

The main function of the Decision part, which is called every iteration and handles the different calls to blocks and functions looks as follows:

   void update() //update checks which action is relevant now
   {
       robot.tresshold_distance=0.75; //sets the threshold every iteration
       if(!scan.update(robot)) {return;} //updates the scan every iteration
       scan.deadend(robot); //starts the check to detect dead ends
       if(!robot.scanRanges.empty())
       {
       //localisation.UpdateKalman(robot); //can be turned on to use the Kalman filter localisation
       }
       scan.open_spaces(robot); //starts the check to detect open spaces
       if(robot.junction == robot.open_space){ //and then acts if the robot is in an open space
           mapping.openSpace();
       }
       if(robot.counter ==0 && robot.deadend_counter ==0) //if the robot is not currently busy handling a situation
       {
           algorithm(); //use the algorithm function to determine it's direction on the next detected situation
       }
       drive.Counter(robot); //updates the counter
       cout<<"Door opened: "<<robot.DoorOpened<<". Waiting for door: "<<robot.WaitingForDoor
          <<". Door-timer: "<<drive.DoorTimer<<endl;
       cout<<"At door area: "<<robot.AtDoorArea<<" Dead-end?: " <<robot.dead_end
           <<" noDoorDeadEnd: "<<robot.NoDoor_DeadEnd<<endl;
       if (robot.ReadyToSendRequest &&
          !robot.NoDoor_DeadEnd &&
          !robot.WaitingForDoor) //condition check for and call to requesting for door
       {
           cout<<"Open the door!!"<<endl;
           drive.WaitToOpenDoor(robot);
       }
       else if (robot.WaitingForDoor) //waiting and checking if the door is opened
       {
           cout<<"Waiting!! Door request"<<endl;
           drive.CheckDoor(robot);
       }
       else if(robot.dead_end==1) //if none of this is met, the robot will turn 180 degrees away from the dead end
       {
           if(robot.collision_check != 0.0)
           {
               drive.CollisionAvoidance(robot);
           }
           else //if scan.door==scan.AtDoorArea &&
           {
               drive.dead_end(robot);
           }
       }
       else if(robot.direction == robot.follow) //follow means that the direction is set to follow a corridor
       {
           if(robot.collision_check != 0.0) //check and call to collision avoidance
           {
               drive.CollisionAvoidance(robot);
           }
           else //driving regular corridor on a potential field
           {
               scan.potential(robot);
               drive.PotentialField_straight_Driving(robot);
           }
       }
       else
       {
           scan.constructing_walls(robot); //builds virtual walls based on the situation
           if(robot.collision_check != 0.0) //check and call to collision avoidance
           {
               drive.CollisionAvoidance(robot);
           }
           else
           {
               scan.potential(robot); //potential field driving with the virtual walls
               if (robot.direction == robot.straight)
               {
                   drive.PotentialField_straight_Driving(robot); //for going straight
               }
               else
               {
                   drive.PotentialFieldDriving(robot); //for taking corners
               }
           }
       }
   }


Within the Decision block there is also the possibility of creating a list of subsequent actions to take. This makes it possible to construct a route to any node in the network set up in the Mapping block. However, since this is not required for Trémaux's algorithm, it is not used. This code would have used entered the current action as the first element of a list, then once the action is over, it would have deleted the first element, making the second element the first element now.

This page is part of the EMC03 CST-wiki.