amiro-os / devices / DiWheelDrive / amiro_map.hpp @ a3c54343
History | View | Annotate | Download (2.947 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 | a3c54343 | galberding | #include <cstdint> |
9 | d02c536e | galberding | #include <math.h> |
10 | e1f1c4b5 | galberding | |
11 | bdac5bec | galberding | |
12 | b8b3a9c9 | galberding | namespace amiro {
|
13 | e1f1c4b5 | galberding | |
14 | 7520e117 | galberding | struct node {
|
15 | uint8_t id; |
||
16 | uint8_t flag; |
||
17 | uint8_t left; |
||
18 | uint8_t right; |
||
19 | uint8_t visited; |
||
20 | types::position pL; // Left
|
||
21 | types::position pR; // Right
|
||
22 | types::position pB; // Back
|
||
23 | }; |
||
24 | |||
25 | struct map_state {
|
||
26 | // 0 - left, 1- right
|
||
27 | uint8_t strategy; |
||
28 | // Node ID of last detected fixpoint
|
||
29 | uint8_t current; |
||
30 | // Next node ID
|
||
31 | uint8_t next; |
||
32 | //Traveled Distance between current and next in %
|
||
33 | bdac5bec | galberding | uint32_t dist; |
34 | 7520e117 | galberding | // True if the current loaded map is valid
|
35 | bool valid;
|
||
36 | bdac5bec | galberding | // Length of the currently traveled edge
|
37 | uint32_t eLength; |
||
38 | 7520e117 | galberding | }; |
39 | e1f1c4b5 | galberding | |
40 | b8b3a9c9 | galberding | class AmiroMap { |
41 | public:
|
||
42 | 7520e117 | galberding | map_state * get_state() { return &state; }
|
43 | a3c54343 | galberding | uint8_t getNodeCount(){ return nodeCount; }
|
44 | node * getNodeList(){return nodeList; }
|
||
45 | |||
46 | b8b3a9c9 | galberding | |
47 | AmiroMap(Global *global) : global{global} {} |
||
48 | |||
49 | /**
|
||
50 | * Initialize a new map from configuration.
|
||
51 | * @param config map configuration array
|
||
52 | *
|
||
53 | */
|
||
54 | 7520e117 | galberding | uint8_t initialize(); |
55 | b8b3a9c9 | galberding | |
56 | |||
57 | /**
|
||
58 | 7520e117 | galberding | * Update the internal map state according to the detected fixpoint
|
59 | * This function should be called for a generic update, each can cycle and in
|
||
60 | * case a fixpoint on one side is detected.
|
||
61 | * Internal state maschine will go into an error state in case both sensors are black.
|
||
62 | *
|
||
63 | * @param left fixpoint on left side detected
|
||
64 | * @param right fixpoint on right side detected
|
||
65 | * @param strategy current line follow strategy
|
||
66 | *
|
||
67 | *
|
||
68 | b8b3a9c9 | galberding | */
|
69 | bdac5bec | galberding | uint8_t update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy); |
70 | b8b3a9c9 | galberding | |
71 | a3c54343 | galberding | |
72 | /**
|
||
73 | If this is called instead of update new fixpoints can automativally
|
||
74 | get detected and will be added to the current nodeList.
|
||
75 | Internally the update will be called.
|
||
76 | If there are no nodes in the node list they will be created.
|
||
77 | */
|
||
78 | uint8_t trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy, ut_states state); |
||
79 | |||
80 | b8b3a9c9 | galberding | private:
|
81 | Global *global; |
||
82 | 7520e117 | galberding | LineFollowStrategy lfStrategy = LineFollowStrategy::EDGE_RIGHT; |
83 | // Keeps track of the internal map state
|
||
84 | map_state state; |
||
85 | // Holds the amount of detected node of the map
|
||
86 | b8b3a9c9 | galberding | uint8_t nodeCount = 0;
|
87 | 7520e117 | galberding | // Stores all nodes and the corresponding coordinates of last detected fixpoints
|
88 | b8b3a9c9 | galberding | node nodeList[MAX_NODES]; |
89 | 7520e117 | galberding | // If driving over fixpoint prevent continuous updating
|
90 | // True if left sensor is driving over black
|
||
91 | bool leftDetected = false; |
||
92 | // True if right sensor is driving over black
|
||
93 | bool rightDetected = false; |
||
94 | |||
95 | /**
|
||
96 | * Recursively search through all nodes in the node list and
|
||
97 | * mark all reached nodes as visited.
|
||
98 | *
|
||
99 | * @param id node from where to start visiting the other nodes
|
||
100 | *
|
||
101 | */
|
||
102 | a07a7a1c | galberding | void visitNode(uint8_t id);
|
103 | bdac5bec | galberding | |
104 | /**
|
||
105 | * Calculate the distance between two given points
|
||
106 | *
|
||
107 | * @param p1 point 1
|
||
108 | * @param p2 point 2
|
||
109 | *
|
||
110 | */
|
||
111 | uint32_t calculateDist(types::position *p1, types::position *p2); |
||
112 | a07a7a1c | galberding | }; |
113 | b8b3a9c9 | galberding | }; // namespace amiro
|
114 | e1f1c4b5 | galberding | |
115 | #endif /* AMIRO_MAP */ |