amiro-os / devices / DiWheelDrive / amiro_map.hpp @ d02c536e
History | View | Annotate | Download (2.502 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 | #include <amiroosconf.h> |
7 | b8b3a9c9 | galberding | #include <ch.hpp> |
8 | d02c536e | galberding | #include <math.h> |
9 | e1f1c4b5 | galberding | |
10 | bdac5bec | galberding | |
11 | b8b3a9c9 | galberding | namespace amiro {
|
12 | e1f1c4b5 | galberding | |
13 | 7520e117 | galberding | 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 | bdac5bec | galberding | uint32_t dist; |
33 | 7520e117 | galberding | // True if the current loaded map is valid
|
34 | bool valid;
|
||
35 | bdac5bec | galberding | // Length of the currently traveled edge
|
36 | uint32_t eLength; |
||
37 | 7520e117 | galberding | }; |
38 | e1f1c4b5 | galberding | |
39 | b8b3a9c9 | galberding | class AmiroMap { |
40 | public:
|
||
41 | 7520e117 | galberding | map_state * get_state() { return &state; }
|
42 | b8b3a9c9 | galberding | |
43 | AmiroMap(Global *global) : global{global} {} |
||
44 | |||
45 | /**
|
||
46 | * Initialize a new map from configuration.
|
||
47 | * @param config map configuration array
|
||
48 | *
|
||
49 | */
|
||
50 | 7520e117 | galberding | uint8_t initialize(); |
51 | b8b3a9c9 | galberding | |
52 | |||
53 | /**
|
||
54 | 7520e117 | galberding | * 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 | b8b3a9c9 | galberding | */
|
65 | bdac5bec | galberding | uint8_t update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy); |
66 | b8b3a9c9 | galberding | |
67 | private:
|
||
68 | Global *global; |
||
69 | 7520e117 | galberding | 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 | b8b3a9c9 | galberding | uint8_t nodeCount = 0;
|
74 | 7520e117 | galberding | // Stores all nodes and the corresponding coordinates of last detected fixpoints
|
75 | b8b3a9c9 | galberding | node nodeList[MAX_NODES]; |
76 | 7520e117 | galberding | // 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 | a07a7a1c | galberding | void visitNode(uint8_t id);
|
90 | bdac5bec | galberding | |
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 | a07a7a1c | galberding | }; |
100 | b8b3a9c9 | galberding | }; // namespace amiro
|
101 | e1f1c4b5 | galberding | |
102 | #endif /* AMIRO_MAP */ |