API tools faq
paste
Advertisement
Guest User

Untitled

a guest
May 27th, 2018
137
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 25.76 KB | None | 0 0
raw download clone embed print report
  1. #include "SnailRunner.h"
  2.  
  3. /*! Anschlüsse des Roboters an den TX Controller.
  4. *  Eingänge:
  5. *  I1 : Farbsensor hinten
  6. *  I4 : Farbsensor unten
  7. *  I6 : Ultraschallsensor
  8. *  I7 : Kontaktsensor
  9. *  I8 : Akkuspannung
  10. *  C1 : Impulszähler rechter Motor
  11. *  C2 : Impulszähler linker Motor
  12. *
  13. *  Ausgänge:
  14. *  M1 : Rechter Motor
  15. *  M2 : Linker Motor
  16. *  O5 : Rechte Lampe
  17. *  O6 : Linke Lampe
  18. *  O7 : Front Lampen
  19. *  O8 : Controller Lampe
  20. */
  21.  
  22. const bool DEBUG_SNAIL = FALSE;
  23.  
  24. #define INPUT_COLOUR_BACK               I1
  25. #define INPUT_COLOUR_DOWN               I4
  26. #define INPUT_ULTRASONIC                I6
  27. #define INPUT_ULTRASONIC_SIDE           I5
  28. #define INPUT_PUSH_BUTTON               I7
  29. #define INPUT_ACCU_LEVEL                I8
  30.  
  31. #define COUNTER_RIGHT_MOTOR             C1
  32. #define COUNTER_LEFT_MOTOR              C2
  33.  
  34. #define OUTPUT_RIGHT_MOTOR              M1
  35. #define OUTPUT_LEFT_MOTOR               M2
  36. #define OUTPUT_LAMP_RIGHT               O5
  37. #define OUTPUT_LAMP_LEFT                O6
  38. #define OUTPUT_LAMPS_FRONT              O7
  39. #define OUTPUT_LAMP_CONTROLLER          O8
  40.  
  41. /* --Diese Informationen sind für die Log-Datei zur Information. */
  42. #define INFO_MOTOR_LEFT     "MOTOR COUNTER|LEFT"
  43. #define INFO_MOTOR_RIGHT    "MOTOR COUNTER|RIGHT"
  44. #define INFO_COLOUR_BACK    "COLOUR|BACK"
  45. #define INFO_COLOUR_DOWN    "COLOUR|FRONT, DOWN"
  46. #define INFO_DISTANCE       "DISTANCE|AHEAD"
  47. #define INFO_DISTANCE_SIDE  "DISTANCE|SIDE"
  48. #define INFO_ACCU_LEVEL     "ANALOG|POWER SUPPLY, ACCU LEVEL"
  49. #define INFO_BUTTON         "BUTTON|PUSH BUTTON, RIGHT"
  50.  
  51. typedef std::tuple<int, int, double> driveDataType;
  52.  
  53. SnailRunner::SnailRunner() : leftmotor(INFO_MOTOR_LEFT), rightmotor(INFO_MOTOR_RIGHT), speed(300),
  54. colourSensorback(INFO_COLOUR_BACK),
  55. colourSensordown(INFO_COLOUR_DOWN),
  56. distance(INFO_DISTANCE),
  57. distanceSide(INFO_DISTANCE_SIDE),
  58. accuLevel(ANALOG_10KV, I1, INFO_ACCU_LEVEL),
  59. pushButton(INFO_BUTTON),
  60. ex_state(0), bm_state(0), st_state(0), ob_state(0), mission(EXPLORE_MISSION) {}
  61.  
  62. SnailRunner::~SnailRunner(void) {
  63.     delete bm_state;
  64.     delete ex_state;
  65.     delete st_state;
  66. }
  67.  
  68. bool SnailRunner::construct(TxControllerSupervision* controller) {
  69.     bool retvalue = Model::construct(controller);
  70.  
  71.     /* --Sensoren konfigurieren. */
  72.     if (retvalue == true && distance.connect(INPUT_ULTRASONIC, controller) == false)
  73.         retvalue = false;
  74.  
  75.     if (retvalue == true && distanceSide.connect(INPUT_ULTRASONIC_SIDE, controller) == false)
  76.         retvalue = false;
  77.  
  78.     if (retvalue == true && colourSensorback.connect(INPUT_COLOUR_BACK, controller) == false)
  79.         retvalue = false;
  80.  
  81.     if (retvalue == true && colourSensordown.connect(INPUT_COLOUR_DOWN, controller) == false)
  82.         retvalue = false;
  83.  
  84.     if (retvalue == true && accuLevel.connect(INPUT_ACCU_LEVEL, controller) == false)
  85.         retvalue = false;
  86.  
  87.  
  88.     if (retvalue == true && pushButton.connect(INPUT_PUSH_BUTTON, controller) == false)
  89.         retvalue = false;
  90.  
  91.     /* --Aktoren konfigurieren. */
  92.     /* --Motoren. */
  93.     if (retvalue == true &&
  94.         (leftmotor.connect(OUTPUT_LEFT_MOTOR, COUNTER_LEFT_MOTOR, controller) == false ||
  95.             rightmotor.connect(OUTPUT_RIGHT_MOTOR, COUNTER_RIGHT_MOTOR, controller) == false))
  96.         retvalue = false;
  97.     else
  98.         /* --Synchronisation der Motoren. */
  99.         leftmotor.synchronize(&rightmotor);
  100.  
  101.     /* --Lampen. */
  102.     if (retvalue == true &&
  103.         (lamp_control.connect(OUTPUT_LAMP_CONTROLLER, controller) == false ||
  104.             lamp_front.connect(OUTPUT_LAMPS_FRONT, controller) == false ||
  105.             lamp_left.connect(OUTPUT_LAMP_LEFT, controller) == false ||
  106.             lamp_right.connect(OUTPUT_LAMP_RIGHT, controller) == false))
  107.         retvalue = false;
  108.  
  109.     /* --Zustandsmaschine erzeugen. */
  110.     ex_state = new ExploreStateMachine(this);
  111.     bm_state = new BrownianMotionStateMachine(this);
  112.     st_state = new StartStateMachine(this);
  113.     ob_state = new ObstacleStateMachine(this);
  114.    
  115.     /* --Interne Attribute setzen. */
  116.     starter = 1; // Start-/Schlusslaeufer ( 1 = Startlaeufer )
  117.     startDirection = 1;
  118.     maxCorners = 8;
  119.     speed = 270;
  120.     last_colour_down = colourdown().value();
  121.     last_dis = ahead().value();
  122.     thresholdColourBlack = 2100;
  123.     thresholdColourGrey = 1800;
  124.     thresholdColourWhite = 900;
  125.     corners = 0;
  126.     rounds = 0;
  127.     maxRounds = 2;
  128.     obstacle = false;
  129.     obstacleTurns = 0;
  130.     driveDistanceImpulse = 0;
  131.     grey = 0;
  132.  
  133.     /* Zeitmessung */
  134.     time_t starttime;
  135.     time_t endtime;
  136.     time(&starttime);
  137.     time(&endtime);
  138.  
  139.     std::list<driveDataType> driveDataList;
  140.  
  141.     //list<double> ColourList = { 2000,2000,2000,2000,2000,2000,2000,2000,2000,2000 };
  142.     double quardSlope = 1;
  143.  
  144.     return retvalue;
  145. }
  146.  
  147. void SnailRunner::saveLogFile() {
  148.     double timediff = difftime(endtime, starttime);
  149. }
  150.  
  151. void SnailRunner::setCorners(int val) {
  152.     corners = val;
  153. }
  154.  
  155. void SnailRunner::setRounds(int val) {
  156.     rounds = val;
  157. }
  158.  
  159. void SnailRunner::setMaxRounds(int val) {
  160.     maxRounds = val;
  161. }
  162.  
  163. int SnailRunner::getRounds() {
  164.     return rounds;
  165. }
  166.  
  167. int SnailRunner::getMaxRounds() {
  168.     return maxRounds;
  169. }
  170.  
  171. void SnailRunner::setThresholdBlack(int val) {
  172.     thresholdColourBlack = val;
  173. }
  174.  
  175. void SnailRunner::setThresholdGrey(int val) {
  176.     thresholdColourGrey = val;
  177. }
  178.  
  179. void SnailRunner::setThresholdWhite(int val) {
  180.     thresholdColourWhite = val;
  181. }
  182.  
  183. int SnailRunner::getThresholdBlack() {
  184.     return thresholdColourBlack;
  185. }
  186.  
  187. int SnailRunner::getThresholdGrey() {
  188.     return thresholdColourGrey;
  189. }
  190.  
  191. int SnailRunner::getThresholdWhite() {
  192.     return thresholdColourWhite;
  193. }
  194.  
  195. void SnailRunner::setStarter(int val) {
  196.     starter = val;
  197. }
  198.  
  199. int SnailRunner::getStarter() {
  200.     return starter;
  201. }
  202.  
  203. void SnailRunner::setObstacleDetected(bool val) {
  204.     obstacle = val;
  205. }
  206.  
  207. bool SnailRunner::getObstacleDetected() {
  208.     return obstacle;
  209. }
  210.  
  211. bool SnailRunner::forward(double val, RunUnit unit) {
  212.     return leftmotor.turnOn(val, unit, speed, LEFT, LEFT);
  213. }
  214.  
  215. bool SnailRunner::backward(double val, RunUnit unit) {
  216.     return leftmotor.turnOn(val, unit, speed, RIGHT, RIGHT);
  217. }
  218.  
  219. bool SnailRunner::turnleft(double val, RunUnit unit) {
  220.     return leftmotor.turnOn(val, unit, speed, LEFT, RIGHT);
  221. }
  222.  
  223. bool SnailRunner::turnright(double val, RunUnit unit) {
  224.     return leftmotor.turnOn(val, unit, speed, RIGHT, LEFT);
  225. }
  226.  
  227. void SnailRunner::setObstacleTurns(int val) {
  228.     obstacleTurns = val;
  229. }
  230.  
  231. int SnailRunner::getObstacleTurns() {
  232.     return obstacleTurns;
  233. }
  234.  
  235. void SnailRunner::setSpeed(int val) {
  236.     speed = val;
  237. }
  238.  
  239. void SnailRunner::addDriveDistance(int val) {
  240.     driveDistanceImpulse += val;
  241. }
  242.  
  243. void SnailRunner::resetDriveDistance() {
  244.     driveDistanceImpulse = 0;
  245. }
  246.  
  247. void SnailRunner::turn(double degree) {
  248.     const double WHEEL_DIAMETER = 0.052;
  249.     const double AXLE_DISTANCE = 0.15;
  250.     const double PI = 3.1415;
  251.     const double GEAR_RATIO = 2.0;
  252.  
  253.     double WHEEL = WHEEL_DIAMETER * PI;
  254.     double CM_IMPULSE = (WHEEL / GEAR_RATIO) / 75;
  255.  
  256.     double path = 0;
  257.     path = ((PI * (AXLE_DISTANCE / 2)) * (abs(degree) / 180));
  258.     /*
  259.     int debug = 0;
  260.     debug = (path / CM_IMPULSE);
  261.     */
  262.     if (degree > 0) {
  263.         turnleft(path / CM_IMPULSE);
  264.         turnright(path / CM_IMPULSE);
  265.     }
  266.     else {
  267.         turnright(path / CM_IMPULSE);
  268.         turnleft(path / CM_IMPULSE);
  269.     }
  270. }
  271. // Maximale Ecken fuer Exploremission eingeben.
  272. void SnailRunner::setMaxCorners(int val) {
  273.     maxCorners = val + 1;
  274. }
  275.  
  276. // Maximale Ecken fuer Exploremission auslesen.
  277. int SnailRunner::getMaxCorners() {
  278.     return maxCorners;
  279. }
  280.  
  281. // Startrichtung fuer Startmission eingeben.
  282. void SnailRunner::setStartDirection(int val) {
  283.     startDirection = val;
  284. }
  285.  
  286. // Startrichtung fuer Startmission auslesen.
  287. int SnailRunner::getStartDirection() {
  288.     return startDirection;
  289. }
  290.  
  291. bool SnailRunner::stop() {
  292.     return leftmotor.turnOff();
  293. }
  294.  
  295. /*! Diese Methode wird aufgerufen, wenn ein oder mehrere Motoren
  296. *  am Roboter gestoppt sind. Dieses kann passieren, wenn (i) der
  297. *  vorgegebene Wert vomn Drehimpulsen erreicht ist, oder (ii) der
  298. *  Motor explizit gestoppt wurde.
  299. *
  300. *  Die Methode überprüft, ob Motor 1 und/oder Motor2 angehalten hat
  301. *  und erzeugt dass Ereignis "IS_STOPPED", welches dann in der
  302. *  Zustandsmaschine zu einer Transition (Zustandswechsel) führt.
  303. *  Das Verhalten des Roboters ist in den Dateien Mission.{cpp|h}
  304. *  beschrieben. */
  305.  
  306. void SnailRunner::onMotorStopped(Bitfield bfield) {
  307.     // --Überprüfe, ob die Motoren gestoppt sind.
  308.     if (bfield&((1 << OUTPUT_LEFT_MOTOR) | (1 << OUTPUT_RIGHT_MOTOR)))
  309.         // --Überprüfe welche Mission.
  310.         if (mission == MOTION_MISSION)
  311.             bm_state->handle(BrownianMotionStateMachine::Event::IS_STOPPED);
  312.         else if (mission == EXPLORE_MISSION)
  313.             ex_state->handle(ExploreStateMachine::Event::IS_STOPPED);
  314.         else if (mission == START_MISSION)
  315.             st_state->handle(StartStateMachine::Event::IS_STOPPED);
  316.         else if (mission == OBSTACLE_MISSION)
  317.             ob_state->handle(ObstacleStateMachine::Event::IS_STOPPED);
  318. }
  319.  
  320. /*! Diese Methode überprüft bestimmte Sensoreingänge. Wenn Schwellenwerte
  321. *  überschritten werden, dann werden Ereignisse erzeugt und diese
  322. *  einer Zustandsmaschine übergeben. In der Zustandsmaschine wird dann
  323. *  eine Transition durchgeführt (in Abhängigkeit der Mission).
  324. *
  325. *  Mission 1 (Spurverfolgung)
  326. *  Wenn der Farbwert (THRESHOLD_COLOR) unterschritten wird, dann wird das
  327. *  Ereignis OFF_TRAIL ausgelöst. Liegt der aktuelle Farbwert wieder über
  328. *  dem Schwellenwert, dann wird das Ereignis ON_TRAIL ausgelöst.
  329. *
  330. *  Mission 2 (Braunsche Bewegung)
  331. *  Wenn der Distanzwert (THRESHOLD_DISTANCE_LOW) unterschritten wird, dann
  332. *  wird das Ereignis WALL_AHEAD ausgelöst. Liegt der aktuelle Distanzwert
  333. *  wieder oberhalb von THRESHOLD_DISTANCE_HIGH, dann wird das Ereignis
  334. *  CLEAR_VIEW ausgelöst.
  335. *
  336. *  Das eigentliche Verhalten des Roboters ist in den Zustandsmaschinen in
  337. *  den Dateien Mission.{cpp|h} beschrieben.
  338. */
  339.  
  340. double calcSlope(list<double> &list) {
  341.     std::list<double>::iterator iterator;
  342.  
  343.         double slope = 0.0;
  344.         double quardSlope = 0.0;
  345.         double sum = 0.0;
  346.  
  347.         if (list.size() >= 5) { list.pop_back(); }
  348.  
  349.         for (iterator = list.begin(); iterator != list.end(); ++iterator) {
  350.             sum += *iterator;
  351.         }
  352.         //slope = (sum / list.size()) / list.front();
  353.         slope = list.front() / list.back();
  354.         quardSlope = slope * slope;
  355.         return quardSlope;
  356. }
  357.  
  358.  
  359.  
  360. void SnailRunner::onInputChanged(Bitfield bfield) {
  361.     int THRESHOLD_COLOR = 2000;
  362.     int THRESHOLD_COLOR_BLACK = this->getThresholdBlack();
  363.     int THRESHOLD_COLOR_GREY = this->getThresholdGrey();
  364.     int THRESHOLD_COLOR_WHITE = this->getThresholdWhite();
  365.     const int THRESHOLD_DISTANCE_LOW = 12;
  366.     const int THRESHOLD_DISTANCE_HIGH = 17;
  367.     const int THRESHOLD_DISTANCE_SIDE_LOW = 7;
  368.     const int THRESHOLD_DISTANCE_SIDE_HIGH = 15;
  369.  
  370.     if (bfield&(1 << INPUT_COLOUR_DOWN)) {
  371.         if (mission == EXPLORE_MISSION) {
  372.             // --Überprüfe, ob Schwellenwert überschritten.
  373.             int col = colourdown().value();
  374.  
  375.             //ColourList.push_front(col);
  376.  
  377.             //quardSlope = calcSlope(ColourList);
  378.            
  379.             //cout << "QUARD STEIGUNG : " << quardSlope << endl;
  380.             //cout << colourdown().value() << endl;
  381.            
  382.  
  383.             /*
  384.             if (quardSlope < 0.6) {
  385.                 ex_state->handle(ExploreStateMachine::Event::ON_TRAIL);
  386.                 cout << endl << "ON_TRAIL" << endl;
  387.             } else if (quardSlope >= 0.6 && quardSlope < 0.8) {
  388.                 ex_state->handle(ExploreStateMachine::Event::GREY_DETECTED);
  389.                 cout << endl << "GREY_DETECTED" << endl;
  390.             } else if (quardSlope >= 0.8 && quardSlope < 1.5) {
  391.             } else if (quardSlope >= 1.5) {
  392.                 ex_state->handle(ExploreStateMachine::Event::OFF_TRAIL);
  393.                 cout << endl << "OFF_TRAIL" << endl;
  394.             }
  395.             */
  396.             /*
  397.             if (quardSlope >= 1.5) {
  398.                 ex_state->handle(ExploreStateMachine::Event::ON_TRAIL);
  399.                 cout << endl << "ON_TRAIL" << endl;
  400.             }
  401.             if (quardSlope < 1.5 && quardSlope >= 0.8 ) {
  402.             }
  403.             if (SnailRunner::ex_state->state() == ExploreStateMachine::State::FOUND && corners >= 1) {
  404.                 if (quardSlope < 0.8 && quardSlope >= 0.55) {
  405.                     if (grey >= 5 && col > THRESHOLD_COLOR_GREY && col < THRESHOLD_COLOR_BLACK) {
  406.                         ex_state->handle(ExploreStateMachine::Event::GREY_DETECTED);
  407.                         cout << endl << "GREY_DETECTED" << endl;
  408.                         grey = 0;
  409.                     }
  410.                     else {
  411.                         grey++;
  412.                     }
  413.                 }
  414.             }
  415.             if (quardSlope < 0.55) {
  416.                 ex_state->handle(ExploreStateMachine::Event::OFF_TRAIL);
  417.                 cout << endl << "OFF_TRAIL" << endl;
  418.             }
  419.             *//*
  420.             if ((quardSlope > 2.5 || quardSlope < 0.85) && (colourdown().value() < THRESHOLD_COLOR_BLACK) && (SnailRunner::ex_state->state() == ExploreStateMachine::State::FOUND) && (corners >= 4)) {
  421.                 ex_state->handle(ExploreStateMachine::Event::GREY_DETECTED);
  422.                 cout << endl << "GREY_DETECTED" << endl;
  423.             } else if (col<THRESHOLD_COLOR_BLACK && last_colour_down>THRESHOLD_COLOR_BLACK) {
  424.                 ex_state->handle(ExploreStateMachine::Event::ON_TRAIL);
  425.                 cout << endl << "ON_TRAIL" << endl;
  426.             } else if (col > THRESHOLD_COLOR_BLACK && last_colour_down < THRESHOLD_COLOR_BLACK) {
  427.                 ex_state->handle(ExploreStateMachine::Event::OFF_TRAIL);
  428.                 cout << endl << "OFF_TRAIL" << endl;
  429.             }
  430.             */
  431.  
  432.         if (col < THRESHOLD_COLOR_BLACK && last_colour_down > THRESHOLD_COLOR_BLACK) {
  433.             ex_state->handle(ExploreStateMachine::Event::ON_TRAIL);
  434.             if (DEBUG_SNAIL) { cout << endl << "ON_TRAIL" << endl; }
  435.         }
  436.         else if (col > THRESHOLD_COLOR_BLACK && last_colour_down < THRESHOLD_COLOR_BLACK) {
  437.             ex_state->handle(ExploreStateMachine::Event::OFF_TRAIL);
  438.             if (DEBUG_SNAIL) { cout << endl << "OFF_TRAIL" << endl; }
  439.         }
  440.             last_colour_down = col;
  441.         }
  442.     }
  443.  
  444.     if (bfield&(1 << INPUT_COLOUR_DOWN)) {
  445.         if (mission == EXPLORE_MISSION) {
  446.             // --Überprüfe, ob Schwellenwert überschritten.
  447.             int col = colourdown().value();
  448.             if (corners >= 4 && (SnailRunner::ex_state->state() == ExploreStateMachine::State::HAND_OVER)) {
  449.                 if (col < THRESHOLD_COLOR_GREY) {
  450.                     cout << col << endl;
  451.                     cout << THRESHOLD_COLOR_GREY << endl;
  452.                     ex_state->handle(ExploreStateMachine::Event::WHITE_DETECTED);
  453.                     if (DEBUG_SNAIL) { cout << endl << "WHITE_DETECTED" << endl; }
  454.                     cout << "White Detected" << endl;
  455.                 }
  456.             }
  457.             last_colour_down = col;
  458.         }
  459.     }
  460.  
  461.     if (bfield&(1 << INPUT_COLOUR_DOWN)) {
  462.         if (mission == OBSTACLE_MISSION) {
  463.             // --Überprüfe, ob Schwellenwert überschritten.
  464.             int col = colourdown().value();
  465.  
  466.  
  467.             if (col < THRESHOLD_COLOR_BLACK && last_colour_down > THRESHOLD_COLOR_BLACK) {
  468.                 ob_state->handle(ObstacleStateMachine::Event::ON_TRAIL);
  469.                 if (DEBUG_SNAIL) { cout << endl << "ON_TRAIL" << endl; }
  470.             }
  471.             else if (col > THRESHOLD_COLOR_BLACK && last_colour_down < THRESHOLD_COLOR_BLACK) {
  472.                 ob_state->handle(ObstacleStateMachine::Event::OFF_TRAIL);
  473.                 if (DEBUG_SNAIL) { cout << endl << "OFF_TRAIL" << endl; }
  474.             }
  475.             last_colour_down = col;
  476.         }
  477.     }
  478.  
  479.     if (bfield&(1 << INPUT_COLOUR_BACK)) {
  480.         if (mission == EXPLORE_MISSION) {
  481.             // --Überprüfe, ob Schwellenwert überschritten.
  482.  
  483.            
  484.             if (colourback().value() < 400) {
  485.                 ex_state->handle(ExploreStateMachine::Event::GET_SIGNAL);
  486.                 if (DEBUG_SNAIL) { cout << endl << "GET_SIGNAL" << endl; }
  487.             }
  488.         }
  489.     }
  490.  
  491.    
  492.     if (bfield&(1 << INPUT_ULTRASONIC)) {
  493.         if (mission == EXPLORE_MISSION) {
  494.             // --Überprüfe, ob Schwellwert überschritten.
  495.             int us = ahead().value();
  496.             //cout << "WALL CORNERS : " << corners << "-----------------------------" << endl;
  497.             if (corners < 4) {
  498.                 if (us >= THRESHOLD_DISTANCE_HIGH && last_dis < THRESHOLD_DISTANCE_HIGH) {
  499.                     ex_state->handle(ExploreStateMachine::Event::CLEAR_VIEW);
  500.                     if (DEBUG_SNAIL) { cout << endl << "CLEAR_VIEW" << endl; }
  501.                 } else if (us < THRESHOLD_DISTANCE_LOW && last_dis >= THRESHOLD_DISTANCE_LOW) {
  502.                     ex_state->handle(ExploreStateMachine::Event::WALL_AHEAD);
  503.                     if (DEBUG_SNAIL) { cout << endl << "WALL_AHEAD" << endl; }
  504.                 }
  505.                 last_dis = us;
  506.             }
  507.         }
  508.     }
  509.  
  510.     if (bfield&(1 << INPUT_ULTRASONIC)) {
  511.         if (mission == EXPLORE_MISSION) {
  512.             // --Überprüfe, ob Schwellwert überschritten.
  513.             int us = ahead().value();
  514.             //cout << "RUNNER  CORNERS : " << corners << "-----------------------------" << endl;
  515.             if (corners >= 4) {
  516.                 if (us >= THRESHOLD_DISTANCE_HIGH && last_dis < THRESHOLD_DISTANCE_HIGH) {
  517.                     ex_state->handle(ExploreStateMachine::Event::RUNNER_CLEAR);
  518.                     if (DEBUG_SNAIL) { cout << endl << "RUNNER_CLEAR" << endl; }
  519.                 } else if (us < THRESHOLD_DISTANCE_LOW && last_dis >= THRESHOLD_DISTANCE_LOW) {
  520.                     ex_state->handle(ExploreStateMachine::Event::RUNNER_AHEAD);
  521.                     if (DEBUG_SNAIL) { cout << endl << "RUNNER_AHEAD" << endl; }
  522.                 }
  523.                 last_dis = us;
  524.             }
  525.         }
  526.     }
  527.    
  528.     if (bfield&(1 << INPUT_ULTRASONIC)) {
  529.         if (mission == START_MISSION) {
  530.             // --Überprüfe, ob Schwellwert überschritten.
  531.             int us = ahead().value();
  532.                 if (us >= THRESHOLD_DISTANCE_HIGH && last_dis < THRESHOLD_DISTANCE_HIGH)
  533.                     st_state->handle(StartStateMachine::Event::CLEAR_VIEW);
  534.                 else if (us < THRESHOLD_DISTANCE_LOW && last_dis >= THRESHOLD_DISTANCE_LOW)
  535.                     st_state->handle(StartStateMachine::Event::WALL_AHEAD);
  536.             last_dis = us;
  537.         }
  538.     }
  539.  
  540.     if (bfield&(1 << INPUT_ULTRASONIC_SIDE)) {
  541.         if (mission == OBSTACLE_MISSION) {
  542.             // --Überprüfe, ob Schwellwert überschritten.
  543.             int us = side().value();
  544.  
  545.             if (us >= THRESHOLD_DISTANCE_SIDE_HIGH && last_dis < THRESHOLD_DISTANCE_SIDE_HIGH) {
  546.  
  547.                 if (getObstacleTurns() < 1) {
  548.                     ob_state->handle(ObstacleStateMachine::Event::SIDE_CLEAR);
  549.                     if (DEBUG_SNAIL) { cout << endl << "SIDE_CLEAR" << endl; }
  550.                 } else {
  551.                     ob_state->handle(ObstacleStateMachine::Event::SIDE_CLEAR_END);
  552.                     if (DEBUG_SNAIL) { cout << endl << "SIDE_CLEAR_END" << endl; }
  553.                     setObstacleTurns(0);
  554.  
  555.                 }
  556.             }
  557.             else if (us < THRESHOLD_DISTANCE_SIDE_LOW && last_dis >= THRESHOLD_DISTANCE_SIDE_LOW) {
  558.                 ob_state->handle(ObstacleStateMachine::Event::SIDE_DETECTED);
  559.                 if (DEBUG_SNAIL) { cout << endl << "SIDE_DETECTED" << endl; }
  560.             }
  561.  
  562.             last_dis = us;
  563.         }
  564.     }
  565.  
  566.     if (bfield&(1 << INPUT_COLOUR_DOWN)) {
  567.         if (mission == OBSTACLE_MISSION) {
  568.             // --Überprüfe, ob Schwellenwert überschritten.
  569.             int ob_col = colourdown().value();
  570.  
  571.             if (ob_col<THRESHOLD_COLOR_BLACK && last_colour_down>THRESHOLD_COLOR_BLACK) {
  572.                 ob_state->handle(ObstacleStateMachine::Event::ON_TRAIL);
  573.                 if (DEBUG_SNAIL) { cout << endl << "ON_TRAIL" << endl; }
  574.             }
  575.             else if (ob_col>THRESHOLD_COLOR_BLACK && last_colour_down<THRESHOLD_COLOR_BLACK) {
  576.                 ob_state->handle(ObstacleStateMachine::Event::OFF_TRAIL);
  577.                 if (DEBUG_SNAIL) { cout << endl << "OFF_TRAIL" << endl; }
  578.             }
  579.             last_colour_down = ob_col;
  580.         }
  581.     }
  582.     if (bfield&(1 << INPUT_COLOUR_DOWN)) {
  583.         if (mission == START_MISSION) {
  584.             // --Überprüfe, ob Schwellenwert überschritten.
  585.             int st_col = colourdown().value();
  586.  
  587.             if (st_col<THRESHOLD_COLOR_BLACK && last_colour_down>THRESHOLD_COLOR_BLACK) {
  588.                 st_state->handle(StartStateMachine::Event::ON_TRAIL);
  589.             }
  590.             else if (st_col>THRESHOLD_COLOR_BLACK && last_colour_down<THRESHOLD_COLOR_BLACK) {
  591.                 st_state->handle(StartStateMachine::Event::OFF_TRAIL);
  592.             }
  593.             last_colour_down = st_col;
  594.         }
  595.     }
  596. }
  597.  
  598. /*! Diese Methode wird zum Beginn des reaktiven Modus des Roboters ausgeführt.
  599. *  Es wird die Controller-Lampe angeschaltet und dann in den ersten Zustand der
  600. *  jeweiligen Zustandsmaschine gewechselt. */
  601.  
  602. void SnailRunner::onStart() {
  603.     lampcontroller().on();
  604.     if (mission == MOTION_MISSION)
  605.         bm_state->start();
  606.     else if (mission == EXPLORE_MISSION)
  607.         ex_state->start();
  608.     else if (mission == START_MISSION)
  609.         st_state->start();
  610.     else if (mission == OBSTACLE_MISSION)
  611.         ob_state->start();
  612. }
  613.  
  614. /*! Diese Methode wird beim Beenden des reaktiven Modus des Roboters ausgeführt.
  615. *  Es wird die blaue Lampe wieder ausgeschaltet. */
  616.  
  617. void SnailRunner::onStop() {
  618.     lampcontroller().off();
  619.     stop();
  620. }
  621.  
  622. void SnailRunner::onCounterChanged(Bitfield /*bfield*/) {}
  623.  
  624.  
  625. void SnailRunner::writeDataToFile(std::ofstream& output, int filenumber) {
  626.     output << "Dateinummer : " << filenumber;
  627. }
  628.  
  629. string SnailRunner::printFileDate() {
  630.     struct tm ltm;
  631.     time_t now = time(0);
  632.     time(&now);
  633.     localtime_s(&ltm, &now);
  634.  
  635.     string time = to_string(ltm.tm_mday) + "." + to_string(ltm.tm_mon) + " - " + to_string(ltm.tm_hour) + ":" + to_string(ltm.tm_min);
  636.  
  637.     return time;
  638. }
  639.  
  640. void SnailRunner::initializeCalibrationFile() {
  641.  
  642.     std::ifstream file("calibration.csv");
  643.  
  644.     if (file.good()) {
  645.         std::cout << "KALIBRIERUNGSDATEI GEFUNDEN" << std::endl;
  646.         std::cout << "KALIBRIERUNGDATEN WERDEN GELESEN" << std::endl << std::endl;
  647.  
  648.         std::string line;
  649.         std::getline(file, line);
  650.         std::getline(file, line);
  651.         setThresholdBlack(atoi(line.c_str()));
  652.         std::getline(file, line);
  653.         std::getline(file, line);
  654.         setThresholdGrey(atoi(line.c_str()));
  655.         std::getline(file, line);
  656.         std::getline(file, line);
  657.         setThresholdWhite(atoi(line.c_str()));
  658.         std::getline(file, line);
  659.         std::getline(file, line);
  660.         setStarter(atoi(line.c_str()));
  661.         std::getline(file, line);
  662.         std::getline(file, line);
  663.         setMaxRounds(atoi(line.c_str()));
  664.         std::getline(file, line);
  665.         std::getline(file, line);
  666.         setStartDirection(atoi(line.c_str()));
  667.     } else {
  668.         std::cout << "KEINE KALIBRIERUNGSDATEI GEFUNDEN" << std::endl;
  669.         std::cout << "NEUE DATEI MIT DEFAULTWERTEN WURDE ERSTELLT" << std::endl;
  670.  
  671.         std::ofstream file("calibration.csv");
  672.         file << "THRESHOLD_BLACK" << std::endl << getThresholdBlack() << std::endl;
  673.         file << "THRESHOLD_GREY" << std::endl << getThresholdGrey() << std::endl;
  674.         file << "THRESHOLD_WHITE" << std::endl << getThresholdWhite() << std::endl;
  675.         file << "STARTER" << std::endl << getStarter() << std::endl;
  676.         file << "ROUNDS" << std::endl << getRounds() << std::endl;
  677.         file << "DIRECTION" << std::endl << getStartDirection() << std::endl;
  678.     }
  679. }
  680.  
  681. std::ofstream SnailRunner::initializeLogFile() {
  682.  
  683.     const std::string filename_prefix = "snail_05_";
  684.     const std::string filetype = ".txt";
  685.     std::string filename = "";
  686.     std::string lastfile = "";
  687.     bool exit = false;
  688.     int filenumber = 1;
  689.  
  690.     filename = filename_prefix + std::to_string(filenumber) + filetype;
  691.  
  692.     if (filenumber <= 9) {
  693.         filename = filename_prefix + "00" + std::to_string(filenumber) + filetype;
  694.     } else if (filenumber <= 99) {
  695.         filename = filename_prefix + "0" + std::to_string(filenumber) + filetype;
  696.     } else {
  697.         filename = filename_prefix + std::to_string(filenumber) + filetype;
  698.     }
  699.  
  700.     // Pruefen ob schon Logdateien vorhanden sind. Wenn ja dann aktuellste ermitteln.
  701.     do {
  702.         std::ifstream file;
  703.         file.open(filename, std::ios::in);
  704.  
  705.         if (file.good()) {
  706.             lastfile = filename;
  707.             filenumber += 1;
  708.  
  709.             if (filenumber <= 9) {
  710.                 filename = filename_prefix + "00" + std::to_string(filenumber) + filetype;
  711.             } else if (filenumber <= 99) {
  712.                 filename = filename_prefix + "0" + std::to_string(filenumber) + filetype;
  713.             } else {
  714.                 filename = filename_prefix + std::to_string(filenumber) + filetype;
  715.             }
  716.         } else {
  717.             exit = true;
  718.             file.close();
  719.  
  720.             if (lastfile.empty()) { lastfile = "keine"; }
  721.             std::cout << "Vorhandene Datei : " << lastfile << std::endl;
  722.             std::cout << "Neue Datei : " << filename << std::endl;
  723.         }
  724.     } while (!exit);
  725.  
  726.     // Werte aus Datei lesen, falls vorhanden.
  727.     if (!lastfile.empty()) {
  728.         std::ifstream input;
  729.         input.open(lastfile, std::ios::in);
  730.  
  731.         std::string inputline;
  732.         std::getline(input, inputline);
  733.  
  734.         std::cout << inputline;
  735.         input.close();
  736.     }
  737.  
  738.     //std::cout << filenumber;
  739.  
  740.     std::ofstream output;
  741.     output.open(filename, std::ios::out);
  742.  
  743.     /*
  744.     if (datei.good()) {
  745.     std::cout << "Datei wurde gefunden... Daten werden gelesen..." << std::endl;
  746.     }
  747.     else {
  748.     std::cout << "Datei nicht vorhanden... wird jetzt erstellt..." << std::endl;
  749.     datei.open(filename, std::ios::out);
  750.     }
  751.     */
  752.  
  753.     return output;
  754. }
  755.  
  756. std::string SnailRunner::printTime() {
  757.     struct tm ltm;
  758.     time_t now = time(0);
  759.     time(&now);
  760.     localtime_s(&ltm, &now);
  761.     string time;
  762.     if (ltm.tm_min <= 9) {
  763.         string time = to_string(ltm.tm_hour) + ":" + "0" + to_string(ltm.tm_min);
  764.         return time;
  765.     } else {
  766.         string time = to_string(ltm.tm_hour) + ":" + to_string(ltm.tm_min);
  767.         return time;
  768.     }
  769.  
  770. }
  771.  
  772. std::string SnailRunner::printDate() {
  773.     struct tm ltm;
  774.     time_t now = time(0);
  775.     time(&now);
  776.     localtime_s(&ltm, &now);
  777.  
  778.     string time = to_string(ltm.tm_mday) + "." + to_string(ltm.tm_mon) + "." + to_string(1900+ltm.tm_year);
  779.  
  780.     return time;
  781. }
  782.  
  783. void SnailRunner::WriteLogFile() {
  784.     cout << "WRITE LOG FILE" << endl;
  785.     /* Uhrzeit und Datum */
  786.     std::ofstream file = initializeLogFile();
  787.     file << std::setw(20) << std::left << "Datum :" << printDate() << std::endl;
  788.     file << std::setw(20) << std::left << "Uhrzeit :" << printTime() << std::endl << std::endl;
  789.  
  790.     /* Fahrtrichtung */
  791.     file << std::setw(20) << std::left << "Fahrtrichtung :";
  792.     /* FAHRTRICHTUNG AUS ROBOTEROBJEKT LADEN*/
  793.     if (getStartDirection() == 1) {
  794.         file << "Rechts";
  795.     } else {
  796.         file << "Links";
  797.     }
  798.     file << std::endl;
  799.     /* Runden */
  800.     file << std::setw(20) << std::left << "Runden :";
  801.     /* RUNDEN AUS ROBOTEROBJEKT AUSLESEN */
  802.     if (true) {
  803.         file << getMaxRounds();
  804.     }
  805.     file << std::endl;
  806.  
  807.     /* Runden */
  808.     file << std::setw(20) << std::left << "Startposition :";
  809.     /* RUNDEN AUS ROBOTEROBJEKT AUSLESEN */
  810.     if (getStarter() == 1) {
  811.         file << "Startlaeufer";
  812.     } else {
  813.         file << "Schlusslaeufer";
  814.     }
  815.     file << std::endl;
  816.  
  817.     /* Runden */
  818.     file << std::setw(20) << std::left << "Hindernisse :";
  819.     /* RUNDEN AUS ROBOTEROBJEKT AUSLESEN */if (true) {
  820.         file << "1 Kleines";
  821.     } else {
  822.         file << "1 Großes";
  823.     }
  824.     file << std::endl << std::endl;
  825.  
  826.     /* Fahrdaten ausgeben */
  827.     file << std::setw(15) << std::left << "Fahrzeit [s]";
  828.     file << std::setw(15) << std::left << "Strecke [cm]";
  829.     file << std::setw(15) << std::left << "Geschwindigkeit [cm/s]" << std::endl;;
  830.  
  831.     file.imbue(std::locale("german")); // Komma statt Dezimalpunkt in der Ausgabe
  832.     if (driveDataList.size() != 0) {
  833.         for (std::list<driveDataType>::iterator iterator = driveDataList.begin(); iterator != driveDataList.end(); iterator++) {
  834.             file << std::setw(15) << std::left << std::get<0>(*iterator);
  835.             file << std::setw(15) << std::left << std::get<1>(*iterator);
  836.             file << std::setw(12) << std::get<2>(*iterator) << std::endl;
  837.         }
  838.     }
  839.  
  840.     file.close();
  841. }
  842.  
  843. void SnailRunner::saveRound() {
  844.     driveDataList.push_back(driveDataType((endtime - starttime), (driveDistanceImpulse / 10.0), (driveDistanceImpulse / 10.0) / (endtime - starttime)));
  845. }
  846.  
  847. void SnailRunner::startTime() {
  848.     starttime = time(0);
  849. }
  850.  
  851. void SnailRunner::stopTime() {
  852.     endtime = time(0);
  853. }
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!