Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / amiro_map.hpp @ d02c536e

History | View | Annotate | Download (2.5 KB)

1
#ifndef AMIRO_MAP
2
#define AMIRO_MAP
3

    
4
#include "global.hpp"
5
#include "linefollow.hpp"
6
#include <amiroosconf.h>
7
#include <ch.hpp>
8
#include <math.h>
9

    
10

    
11
namespace amiro {
12

    
13
  struct node {
14
    uint8_t id;
15
    uint8_t flag;
16
    uint8_t left;
17
    uint8_t right;
18
    uint8_t visited;
19
    types::position pL; // Left
20
    types::position pR; // Right
21
    types::position pB; // Back
22
  };
23

    
24
  struct map_state {
25
    // 0 - left, 1- right
26
    uint8_t strategy;
27
    // Node ID of last detected fixpoint
28
    uint8_t current;
29
    // Next node ID
30
    uint8_t next;
31
    //Traveled Distance between current and next in %
32
    uint32_t dist;
33
    // True if the current loaded map is valid
34
    bool valid;
35
    // Length of the currently traveled edge
36
    uint32_t eLength;
37
  };
38

    
39
class AmiroMap {
40
public:
41
  map_state * get_state() { return &state; }
42

    
43
  AmiroMap(Global *global) : global{global} {}
44

    
45
  /**
46
   * Initialize a new map from configuration.
47
   * @param config map configuration array
48
   *
49
   */
50
  uint8_t initialize();
51

    
52

    
53
  /**
54
   * Update the internal map state according to the detected fixpoint
55
   * This function should be called for a generic update, each can cycle and in
56
   * case a fixpoint on one side is detected.
57
   * Internal state maschine will go into an error state in case both sensors are black.
58
   *
59
   *  @param left fixpoint on left side detected
60
   *  @param right fixpoint on right side detected
61
   *  @param strategy current line follow strategy
62
   *
63
   *
64
   */
65
  uint8_t update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy);
66

    
67
private:
68
  Global *global;
69
  LineFollowStrategy lfStrategy = LineFollowStrategy::EDGE_RIGHT;
70
  // Keeps track of the internal map state
71
  map_state state;
72
  // Holds the amount of detected node of the map
73
  uint8_t nodeCount = 0;
74
  // Stores all nodes and the corresponding coordinates of last detected fixpoints
75
  node nodeList[MAX_NODES];
76
  // If driving over fixpoint prevent continuous updating
77
  // True if left sensor is driving over black
78
  bool leftDetected = false;
79
  // True if right sensor is driving over black
80
  bool rightDetected = false;
81

    
82
  /**
83
   * Recursively search through all nodes in the node list and
84
   * mark all reached nodes as visited.
85
   *
86
   * @param id node from where to start visiting the other nodes
87
   *
88
   */
89
  void visitNode(uint8_t id);
90

    
91
  /**
92
   *  Calculate the distance between two given points
93
   *
94
   *  @param p1 point 1
95
   *  @param p2 point 2
96
   *
97
   */
98
  uint32_t calculateDist(types::position *p1, types::position *p2);
99
  };
100
}; // namespace amiro
101

    
102
#endif /* AMIRO_MAP */