API tools faq
paste
Advertisement
Guest User

Untitled

a guest
May 27th, 2018
64
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 14.64 KB | None | 0 0
raw download clone embed print report
  1. #include "ExploreMission.h"
  2. #include "SnailRunner.h"
  3.  
  4. /* --Initialize State Description. */
  5. const bool DEBUG_EXP = FALSE;
  6. const std::map<ExploreStateMachine::State, std::string> ExploreStateMachine::StateDescription = {
  7.     { State::FAILURE, "FAILURE" },
  8.     { State::FOUND, "FOUND" },
  9.     { State::LOST, "LOST" },
  10.     { State::LOOK_LEFT, "LOOK_LEFT" },
  11.     { State::LOOK_RIGHT, "LOOK_RIGHT" },
  12.     { State::STOPPING, "STOPPING" },
  13.     { State::STOPPING_GREY, "STOPPING_GREY" },
  14.     { State::WAIT, "WAIT"},
  15.     { State::HAND_OVER, "HAND_OVER" },
  16.     { State::DELIVERY, "DELIVERY" },
  17.     { State::FINISH, "FINISH" },
  18.     { State::OBSTACLE, "OBSTACLE" }
  19.     /* --INFO: Here you should add new states for debugging purpose. */
  20. };
  21.  
  22. const std::map<ExploreStateMachine::Event, std::string> ExploreStateMachine::EventDescription = {
  23.     { Event::IS_STOPPED, "IS_STOPPED" },
  24.     { Event::ON_TRAIL, "ON_TRAIL" },
  25.     { Event::OFF_TRAIL, "OFF_TRAIL" },
  26.     { Event::MAX_CORNERS, "MAX_CORNERS" },
  27.     { Event::WALL_AHEAD, "WALL_AHEAD" },
  28.     { Event::GET_SIGNAL, "GET_SIGNAL" },
  29.     { Event::RUNNER_CLEAR, "RUNNER_CLEAR" },
  30.     { Event::WHITE_DETECTED, "WHITE_DETECTED" },
  31.     { Event::RUNNER_AHEAD, "RUNNER_AHEAD" }
  32.     /* --INFO: Here you should add new events for debugging purpose. */
  33. };
  34.  
  35.  
  36. ExploreStateMachine::ExploreStateMachine(SnailRunner* r) :mystate(State::FAILURE), robot(r), count(1) {}
  37.  
  38.  
  39. /* --INFO: Do not touch this method. */
  40. void ExploreStateMachine::state(State s) {
  41.     /* --Log state chance. Check if description is available. */
  42.     bool isOldStateAvailable = (StateDescription.find(mystate) != StateDescription.end());
  43.     bool isNewStateAvailable = (StateDescription.find(s) != StateDescription.end());
  44.     /* --If program stops here, you have to add a state description to the map at top of this file !!! */
  45.     if (!isOldStateAvailable && !isNewStateAvailable)
  46.         OutputDebugString((LPCTSTR)"*** CHECK YOUR CODE! STATE DESCRIPTION IS MISSING ***");
  47.  
  48.     /* --Log string. */
  49.     std::string note = std::string("OLD STATE:") + (isOldStateAvailable ? StateDescription.at(mystate) : "*ERROR*")
  50.         + std::string(" --> NEW STATE:") + (isNewStateAvailable ? StateDescription.at(s) : "*ERROR*");
  51.     robot->controller()->annotate(note);
  52.  
  53.     /* --Set new state.*/
  54.     mystate = s;
  55. }
  56.  
  57. /* --INFO: Do not touch this method. */
  58. ExploreStateMachine::State ExploreStateMachine::state() const { return mystate; }
  59.  
  60. void ExploreStateMachine::start() {
  61.     /* --Log string. */
  62.     robot->controller()->annotate("RE/START EXPLORE STATE MACHINE");
  63.     /* --Set everything to initial values. */
  64.     count = 1;
  65.     /* --Start with initial transition. */
  66.  
  67.     robot->lampright().off();
  68.     robot->lampfront().off();
  69.     if (!(robot->getObstacleDetected())) {
  70.         if (robot->getStarter() == 1) {
  71.             robot->lampright().on();
  72.             onEnteringFound();
  73.             corners = 0;
  74.             if (DEBUG_EXP) { cout << endl << "STARTLAEUFER EXP" << endl; }
  75.         } else {
  76.             onEnteringDelivery();
  77.             corners = 0;
  78.             robot->lampright().off();
  79.             if (DEBUG_EXP) { cout << endl << "SCHLUSSLAEUFER EXP" << endl; }
  80.         }
  81.     } else {
  82.         if (DEBUG_EXP) { cout << endl << "HINDERNIS WURDE UMFAHREN EXP" << endl; }
  83.         onEnteringFound();
  84.         corners = 2;
  85.         robot->setObstacleDetected(false);
  86.     }
  87.     /* --INFO: Here you can change/add initial values when state machine is started. */
  88. }
  89.  
  90.  
  91. void ExploreStateMachine::restart() {
  92.     /* --Same as start. */
  93.     start();
  94. }
  95.  
  96. /* --INFO: Do not touch this method. */
  97. void ExploreStateMachine::handle(Event ev) {
  98.     State old = mystate;
  99.  
  100.     /* --Logging information. */
  101.     bool isEventAvailable = (EventDescription.find(ev) != EventDescription.end());
  102.     /* --If program stops here, you have to add an event description to the map at top of this file !!! */
  103.     if (!isEventAvailable)
  104.         OutputDebugString((LPCTSTR)"*** CHECK YOUR CODE! EVENT DESCRIPTION IS MISSING ***");
  105.  
  106.     /* --Log string. */
  107.     std::string note = std::string("EVENT TRIGGER:") + (isEventAvailable ? EventDescription.at(ev) : "*ERROR*");
  108.     robot->controller()->annotate(note);
  109.  
  110.     /* --Process the event. */
  111.     transition(ev);
  112.    
  113.     /* --Check if state was changed. Could be an issue with your state machine.*/
  114.     if (old == mystate)
  115.         robot->controller()->annotate(std::string("MAYBE UNHANDLED TRIGGER!"));
  116. }
  117.  
  118. /* --INFO: This is the implemented state machine. Add/Modify/Enhance this method for additional functionality. */
  119. void ExploreStateMachine::transition(Event ev) {
  120.     /* --State machine as nested switch case. */
  121.     switch (mystate) {
  122.     case State::FOUND:
  123.         switch (ev) {
  124.         case Event::IS_STOPPED: onLeavingFound(); onEnteringFound(); break;
  125.         case Event::ON_TRAIL: break;
  126.         case Event::OFF_TRAIL: onLeavingFound(); onEnteringLost(); break;
  127.         case Event::CLEAR_VIEW: break;
  128.         case Event::WALL_AHEAD: onLeavingFound(); onEnteringObstacle(); break;
  129.         case Event::RUNNER_AHEAD: onLeavingFound(); onEnteringHandOver(); break;
  130.         case Event::GET_SIGNAL: break;
  131.         case Event::WHITE_DETECTED: break;
  132.         case Event::RUNNER_CLEAR: break;
  133.         default: onLeavingFound(); onEnteringFailure();
  134.         }
  135.         break; 
  136.     case State::HAND_OVER:
  137.         switch (ev) {
  138.         case Event::IS_STOPPED: break;
  139.         case Event::ON_TRAIL: break;
  140.         case Event::OFF_TRAIL: break;
  141.         case Event::WALL_AHEAD: break;
  142.         case Event::GET_SIGNAL: break;
  143.         case Event::RUNNER_AHEAD: onLeavingHandOver(); onEnteringStoppingGrey(); break;
  144.         case Event::WHITE_DETECTED: onLeavingHandOver(); onEnteringDelivery(); break;
  145.         case Event::RUNNER_CLEAR: break;
  146.         default: onLeavingHandOver(); onEnteringFailure();
  147.         }
  148.         break;
  149.     case State::LOST:
  150.         switch (ev) {
  151.         case Event::IS_STOPPED: onLeavingLost(); onEnteringLookLeft(); break;
  152.         case Event::ON_TRAIL: break;
  153.         case Event::OFF_TRAIL: break;
  154.         case Event::WALL_AHEAD: break;
  155.         case Event::RUNNER_AHEAD: break;
  156.         case Event::GET_SIGNAL: break;
  157.         case Event::WHITE_DETECTED: break;
  158.         case Event::RUNNER_CLEAR: break;
  159.         default: onLeavingLost(); onEnteringFailure();
  160.         }
  161.         break;
  162.     case State::DELIVERY:
  163.         switch (ev) {
  164.         case Event::IS_STOPPED: break;
  165.         case Event::ON_TRAIL: break;
  166.         case Event::OFF_TRAIL: break;
  167.         case Event::WALL_AHEAD: break;
  168.         case Event::RUNNER_AHEAD: break;
  169.         case Event::CLEAR_VIEW: break;
  170.         case Event::WHITE_DETECTED: break;
  171.         case Event::RUNNER_CLEAR: break;
  172.         case Event::GET_SIGNAL: onLeavingDelivery(); onEnteringFound(); break;
  173.         default: onLeavingDelivery(); onEnteringFailure();
  174.         }
  175.         break;
  176.     case State::OBSTACLE:
  177.         switch (ev) {
  178.         case Event::IS_STOPPED: break;
  179.         case Event::ON_TRAIL: break;
  180.         case Event::OFF_TRAIL: break;
  181.         case Event::WALL_AHEAD: break;
  182.         case Event::RUNNER_AHEAD: break;
  183.         case Event::WHITE_DETECTED: break;
  184.         case Event::GET_SIGNAL: break;
  185.         case Event::RUNNER_CLEAR: break;
  186.         //case Event::CLEAR_VIEW: onLeavingObstacle(); onEnteringFound(); break;
  187.         default: onLeavingObstacle(); onEnteringFailure();
  188.         }
  189.         break;
  190.     case State::LOOK_LEFT:
  191.         switch (ev) {
  192.         case Event::IS_STOPPED: onLeavingLookLeft(); onEnteringLookRight();  break;
  193.         case Event::ON_TRAIL: onLeavingLookLeft(); onEnteringStopping();  break;
  194.         case Event::OFF_TRAIL: break;
  195.         case Event::CLEAR_VIEW: break;
  196.         case Event::WALL_AHEAD: break;
  197.         case Event::RUNNER_AHEAD: break;
  198.         case Event::WHITE_DETECTED: break;
  199.         case Event::GET_SIGNAL: break;
  200.         case Event::RUNNER_CLEAR: break;
  201.         default: onLeavingLookLeft(); onEnteringFailure();
  202.         }
  203.         break;
  204.     case State::LOOK_RIGHT:
  205.         switch (ev) {
  206.         case Event::IS_STOPPED: onLeavingLookRight(); onEnteringLookLeft();  break;
  207.         case Event::ON_TRAIL: onLeavingLookRight(); onEnteringStopping();  break;
  208.         case Event::OFF_TRAIL: break;
  209.         case Event::CLEAR_VIEW: break;
  210.         case Event::WALL_AHEAD: break;
  211.         case Event::WHITE_DETECTED: break;
  212.         case Event::RUNNER_AHEAD: break;
  213.         case Event::GET_SIGNAL: break;
  214.         case Event::RUNNER_CLEAR: break;
  215.         default: onLeavingLookRight(); onEnteringFailure();
  216.         }
  217.         break;
  218.     case State::STOPPING:
  219.         switch (ev) {
  220.         case Event::IS_STOPPED: onLeavingStopping(); onEnteringFound(); break;
  221.         case Event::ON_TRAIL: break;
  222.         case Event::OFF_TRAIL: break;
  223.         case Event::CLEAR_VIEW: break;
  224.         case Event::WALL_AHEAD: break;
  225.         case Event::WHITE_DETECTED: break;
  226.         case Event::GET_SIGNAL: break;
  227.         case Event::RUNNER_AHEAD: break;
  228.         case Event::RUNNER_CLEAR: onLeavingStopping(); onEnteringHandOver(); break;
  229.         default: onLeavingStopping(); onEnteringFailure();
  230.         }
  231.         break;
  232.     case State::STOPPING_GREY:
  233.         switch (ev) {
  234.         case Event::IS_STOPPED: break;
  235.         case Event::ON_TRAIL: break;
  236.         case Event::OFF_TRAIL: break;
  237.         case Event::CLEAR_VIEW: break;
  238.         case Event::GET_SIGNAL: break;
  239.         case Event::WALL_AHEAD: break;
  240.         case Event::WHITE_DETECTED: break;
  241.         case Event::RUNNER_AHEAD: break;
  242.         case Event::RUNNER_CLEAR: onLeavingStopping(); onEnteringHandOver(); break;
  243.         default: onLeavingStopping(); onEnteringFailure();
  244.         }
  245.         break;
  246.     case State::WAIT:
  247.         switch (ev) {
  248.         case Event::IS_STOPPED: break;
  249.         case Event::ON_TRAIL: break;
  250.         case Event::OFF_TRAIL: break;
  251.         case Event::WALL_AHEAD: break;
  252.         case Event::GET_SIGNAL: break;
  253.         case Event::WHITE_DETECTED: break;
  254.         case Event::RUNNER_AHEAD: break;
  255.         case Event::RUNNER_CLEAR: break;
  256.         default: onLeavingWait(); onEnteringFailure();
  257.         }
  258.         break;
  259.     case State::FINISH:
  260.         switch (ev) {
  261.         case Event::IS_STOPPED: break;
  262.         case Event::ON_TRAIL: break;
  263.         case Event::OFF_TRAIL: break;
  264.         case Event::WALL_AHEAD: break;
  265.         case Event::GET_SIGNAL: break;
  266.         case Event::WHITE_DETECTED: break;
  267.         case Event::RUNNER_AHEAD: break;
  268.         case Event::RUNNER_CLEAR: break;
  269.         default: onLeavingWait(); onEnteringFailure();
  270.         }
  271.         break;
  272.     default: state(State::FAILURE);
  273.     };
  274. }
  275.  
  276. /* --INFO: These methods can be changed to add/modify the given behavior. Important: The
  277.            first instruction in all onEnteringXYZ() methods should call state(State::XYZ)
  278.            in order to set the new/current state. */
  279. void ExploreStateMachine::onEnteringFailure() {
  280.     state(State::FAILURE);
  281.     robot->stop();
  282.     robot->lampleft().on();
  283. }
  284.  
  285. void ExploreStateMachine::onEnteringFound() {
  286.     robot->forward(1, METER);
  287.     state(State::FOUND);
  288.     robot->lampright().on();
  289.     robot->setSpeed(270);
  290.     // Pruefen ob eine Ecke erkannt worden ist. Wenn ja dann Event MAX_CORNERS ausloesen.
  291.     if (count >= 30) {
  292.         corners += 1;
  293.         robot->setCorners(corners);
  294.         cout << endl << "ECKE ERKANNT : " << corners << endl;
  295.         if (corners >= robot->getMaxCorners()) {
  296.             this->handle(ExploreStateMachine::Event::MAX_CORNERS);
  297.             corners = 0;
  298.         }
  299.     }
  300.     count = 1;
  301. }
  302. void ExploreStateMachine::onEnteringWait() {
  303.     state(State::WAIT);
  304.     if (DEBUG_EXP) { cout << endl << "ENTERING WAIT EXP" << endl; }
  305.     robot->stop();
  306. }
  307.  
  308. void ExploreStateMachine::onEnteringLost() {
  309.     state(State::LOST);
  310.     if (DEBUG_EXP) { cout << endl << "ENTERING LOST EXP" << endl; }
  311.     robot->stop();
  312. }
  313.  
  314. void ExploreStateMachine::onEnteringLookLeft() {
  315.     state(State::LOOK_LEFT);
  316.     if (DEBUG_EXP) { cout << endl << "ENTERING LOOK LEFT EXP" << endl; }
  317.     robot->turn(count * -3
  318.     );
  319.     count += 3;
  320.     if (count > 12) { count += 5; }
  321.     if (count >= 9) { robot->setSpeed(400); }
  322. }
  323.  
  324. void ExploreStateMachine::onEnteringLookRight() {
  325.     state(State::LOOK_RIGHT);
  326.     if (DEBUG_EXP) { cout << endl << "ENTERING LOOK RIGHT EXP" << endl; }
  327.     robot->turn(count * 3);
  328.     count += 3;
  329.     cout << count << endl;
  330.     if (count > 12) { count += 5; }
  331.     if (count >= 9) { robot->setSpeed(400); }
  332. }
  333.  
  334. void ExploreStateMachine::onEnteringStopping() {
  335.     state(State::STOPPING);
  336.     robot->stop();
  337.     if (DEBUG_EXP) { cout << endl << "ENTERING STOPPING EXP" << endl; }
  338. }
  339.  
  340. void ExploreStateMachine::onLeavingFailure() {
  341.     robot->lampright().off();
  342.     if (DEBUG_EXP) { cout << endl << "ENTERING FAILURE EXP" << endl; }
  343. }
  344.  
  345. void ExploreStateMachine::onLeavingFound() {
  346.     if (DEBUG_EXP) { cout << endl << "ENCODER LINKS " << robot->left().encoder().value() << endl; }
  347.     if (DEBUG_EXP) { cout << endl << "ENCODER RECHTS " << robot->right().encoder().value() << endl; }
  348.     robot->addDriveDistance(robot->left().encoder().value());
  349.     cout << "FAHRSTRECKE FOUND" << endl;
  350.     if (DEBUG_EXP) { cout << endl << "LEAVING FOUND EXP" << endl; }
  351. }
  352.  
  353. void ExploreStateMachine::onLeavingLost() {
  354.     if (DEBUG_EXP) { cout << endl << "LEAVING LOST EXP" << endl; }
  355. }
  356.  
  357. void ExploreStateMachine::onLeavingStopping() {
  358.     robot->setSpeed(270);
  359.     if (DEBUG_EXP) { cout << endl << "LEAVING STOPPING EXP" << endl; }
  360. }
  361.  
  362. void ExploreStateMachine::onLeavingLookLeft() {
  363.     if (DEBUG_EXP) { cout << endl << "LEAVING LOOK LEFT EXP" << endl; }
  364. }
  365.  
  366. void ExploreStateMachine::onLeavingLookRight() {
  367.     if (DEBUG_EXP) { cout << endl << "LEAVING LOOK RIGHT EXP" << endl; }
  368. }
  369.  
  370. void ExploreStateMachine::onLeavingWait() {
  371.     if (DEBUG_EXP) { cout << endl << "LEAVING WAIT EXP" << endl; }
  372. }
  373.  
  374. void ExploreStateMachine::onEnteringHandOver() {
  375.     state(State::HAND_OVER);
  376.     robot->stopTime();
  377.     robot->setRounds(robot->getRounds() + 1);
  378.     robot->forward(0.5, METER);
  379.     robot->lampfront().on();
  380.     robot->lampright().off();
  381.     if (DEBUG_EXP) { cout << endl << "ENTERING HANDOVER EXP" << endl; }
  382. }
  383.  
  384. void ExploreStateMachine::onLeavingHandOver() {
  385.     if (DEBUG_EXP) { cout << endl << "LEAVING HANDOVER EXP" << endl; }
  386. }
  387.  
  388. void ExploreStateMachine::onEnteringDelivery() {
  389.     if (DEBUG_EXP) { cout << endl << "ENTERING DELIVERY EXP" << endl; }
  390.     state(State::DELIVERY);
  391.     robot->stop();
  392.     robot->setCorners(0);
  393.     robot->saveRound();
  394.     robot->resetDriveDistance();
  395.     if (robot->getRounds() == robot->getMaxRounds()) {
  396.         cout << endl << "STAFFELLAUF BEENDET !!!!!!!!!!!!!!!!!!!!!!" << endl;
  397.         state(State::FINISH);
  398.         robot->WriteLogFile();
  399.         robot->lampfront().off();
  400.         robot->lampright().off();
  401.         robot->lampcontroller().off();
  402.     }
  403. }
  404.  
  405. void ExploreStateMachine::onLeavingDelivery() {
  406.     robot->activate(SnailRunner::EXPLORE_MISSION);
  407.     robot->startTime();
  408.     robot->onStart();
  409.     robot->lampfront().off();
  410.     if (DEBUG_EXP) { cout << endl << "LEAVING DELIVERY EXP" << endl; }
  411. }
  412.  
  413. void ExploreStateMachine::onEnteringObstacle() {
  414.     state(State::OBSTACLE);
  415.     robot->setObstacleDetected(true);
  416.     robot->activate(SnailRunner::OBSTACLE_MISSION);
  417.     robot->onStart();
  418.     if (DEBUG_EXP) { cout << endl << "ENTERING OBSTACLE EXP" << endl; }
  419. }
  420.  
  421. void ExploreStateMachine::onLeavingObstacle() {
  422.     robot->lampfront().off();
  423.     if (DEBUG_EXP) { cout << endl << "LEAVING OBSTACLE EXP" << endl; }
  424. }
  425.  
  426. void ExploreStateMachine::onLeavingStoppingGrey() {
  427.     if (DEBUG_EXP) { cout << endl << "LEAVING STOPPING GREY EXP" << endl; }
  428. }
  429.  
  430. void ExploreStateMachine::onEnteringStoppingGrey() {
  431.     state(State::STOPPING_GREY);
  432.     robot->stop();
  433.     if (DEBUG_EXP) { cout << endl << "ENTERING STOPPING GREY EXP" << endl; }
  434. }
  435.  
  436. void ExploreStateMachine::onLeavingFinish() {
  437.     if (DEBUG_EXP) { cout << endl << "LEAVING FINISH EXP" << endl; }
  438. }
  439.  
  440. void ExploreStateMachine::onEnteringFinish() {
  441.     state(State::FINISH);
  442.     cout << "STAFFELLAUF BEENDET !!! - ZUSTAND FNIISH" << endl;
  443.     robot->stop();
  444.     if (DEBUG_EXP) { cout << endl << "ENTERING FINISH EXP" << endl; }
  445. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!