amiro-os / devices / DiWheelDrive / amiro_map.hpp @ f1d13b04
History | View | Annotate | Download (3.402 KB)
1 | e1f1c4b5 | galberding | #ifndef AMIRO_MAP
|
---|---|---|---|
2 | #define AMIRO_MAP
|
||
3 | |||
4 | #include "global.hpp" |
||
5 | b8b3a9c9 | galberding | #include "linefollow.hpp" |
6 | e1f1c4b5 | galberding | |
7 | bdac5bec | galberding | |
8 | b8b3a9c9 | galberding | namespace amiro {
|
9 | e1f1c4b5 | galberding | |
10 | 7520e117 | galberding | struct node {
|
11 | uint8_t id; |
||
12 | uint8_t flag; |
||
13 | uint8_t left; |
||
14 | uint8_t right; |
||
15 | 3aee55de | galberding | union edge {
|
16 | struct edge_id{
|
||
17 | uint8_t left; |
||
18 | uint8_t right; |
||
19 | } edge_id; |
||
20 | uint8_t arr[2];
|
||
21 | } edge; |
||
22 | 7520e117 | galberding | uint8_t visited; |
23 | types::position pL; // Left
|
||
24 | types::position pR; // Right
|
||
25 | types::position pB; // Back
|
||
26 | 3aee55de | galberding | union p{
|
27 | struct{types::position pL, pR, pB;} points;
|
||
28 | types::position arr[3];
|
||
29 | } p; |
||
30 | 7520e117 | galberding | }; |
31 | |||
32 | e1f1c4b5 | galberding | |
33 | b8b3a9c9 | galberding | class AmiroMap { |
34 | public:
|
||
35 | 7520e117 | galberding | map_state * get_state() { return &state; }
|
36 | a3c54343 | galberding | uint8_t getNodeCount(){ return nodeCount; }
|
37 | node * getNodeList(){return nodeList; }
|
||
38 | |||
39 | b8b3a9c9 | galberding | |
40 | AmiroMap(Global *global) : global{global} {} |
||
41 | |||
42 | /**
|
||
43 | * Initialize a new map from configuration.
|
||
44 | * @param config map configuration array
|
||
45 | *
|
||
46 | */
|
||
47 | 7520e117 | galberding | uint8_t initialize(); |
48 | b8b3a9c9 | galberding | |
49 | |||
50 | /**
|
||
51 | 7520e117 | galberding | * Update the internal map state according to the detected fixpoint
|
52 | * This function should be called for a generic update, each can cycle and in
|
||
53 | * case a fixpoint on one side is detected.
|
||
54 | * Internal state maschine will go into an error state in case both sensors are black.
|
||
55 | *
|
||
56 | * @param left fixpoint on left side detected
|
||
57 | * @param right fixpoint on right side detected
|
||
58 | * @param strategy current line follow strategy
|
||
59 | *
|
||
60 | *
|
||
61 | b8b3a9c9 | galberding | */
|
62 | bdac5bec | galberding | uint8_t update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy); |
63 | b8b3a9c9 | galberding | |
64 | a3c54343 | galberding | |
65 | /**
|
||
66 | If this is called instead of update new fixpoints can automativally
|
||
67 | get detected and will be added to the current nodeList.
|
||
68 | Internally the update will be called.
|
||
69 | If there are no nodes in the node list they will be created.
|
||
70 | */
|
||
71 | 3aee55de | galberding | uint8_t trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy, ut_states ut_state); |
72 | |||
73 | |||
74 | |||
75 | |||
76 | a3c54343 | galberding | |
77 | b8b3a9c9 | galberding | private:
|
78 | Global *global; |
||
79 | 7520e117 | galberding | LineFollowStrategy lfStrategy = LineFollowStrategy::EDGE_RIGHT; |
80 | // Keeps track of the internal map state
|
||
81 | map_state state; |
||
82 | // Holds the amount of detected node of the map
|
||
83 | b8b3a9c9 | galberding | uint8_t nodeCount = 0;
|
84 | 7520e117 | galberding | // Stores all nodes and the corresponding coordinates of last detected fixpoints
|
85 | b8b3a9c9 | galberding | node nodeList[MAX_NODES]; |
86 | 7520e117 | galberding | // If driving over fixpoint prevent continuous updating
|
87 | // True if left sensor is driving over black
|
||
88 | bool leftDetected = false; |
||
89 | // True if right sensor is driving over black
|
||
90 | bool rightDetected = false; |
||
91 | |||
92 | /**
|
||
93 | * Recursively search through all nodes in the node list and
|
||
94 | * mark all reached nodes as visited.
|
||
95 | *
|
||
96 | * @param id node from where to start visiting the other nodes
|
||
97 | *
|
||
98 | */
|
||
99 | a07a7a1c | galberding | void visitNode(uint8_t id);
|
100 | bdac5bec | galberding | |
101 | /**
|
||
102 | * Calculate the distance between two given points
|
||
103 | *
|
||
104 | * @param p1 point 1
|
||
105 | * @param p2 point 2
|
||
106 | *
|
||
107 | */
|
||
108 | uint32_t calculateDist(types::position *p1, types::position *p2); |
||
109 | 3aee55de | galberding | |
110 | // add node/fxp to the nodeList and return its id
|
||
111 | uint8_t addNode(uint8_t l, uint8_t r, uint8_t flags); |
||
112 | |||
113 | // Calculate dist and elength if possibel
|
||
114 | 81b48c66 | galberding | void calTravelState(types::position *p1);
|
115 | 3aee55de | galberding | |
116 | // Check if all nodes have followers
|
||
117 | // Calculate validity check
|
||
118 | void checkMap();
|
||
119 | |||
120 | void switchToNext(types::position *p1);
|
||
121 | |||
122 | // Copy contents of point from to point to (x, y, f_x)
|
||
123 | void copyPoint(types::position *from, types::position *to);
|
||
124 | |||
125 | // Create the first fxp with flag 1
|
||
126 | void createInitNode();
|
||
127 | |||
128 | uint8_t getNearest(types::position *p1); |
||
129 | |||
130 | // Either create new fxpoint or assign point to existing one
|
||
131 | uint8_t assignFxp(types::position *p1); |
||
132 | f1d13b04 | galberding | void reset();
|
133 | 3aee55de | galberding | }; |
134 | |||
135 | b8b3a9c9 | galberding | }; // namespace amiro
|
136 | e1f1c4b5 | galberding | |
137 | #endif /* AMIRO_MAP */ |