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