amiro-os / devices / DiWheelDrive / amiro_map.hpp @ d2230e6e
History | View | Annotate | Download (1.83 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 | #include <cstdint> |
||
9 | e1f1c4b5 | galberding | |
10 | b8b3a9c9 | galberding | #define MAX_NODES 10 |
11 | #define NODE_ATTRIBUTES 3 |
||
12 | e1f1c4b5 | galberding | |
13 | b8b3a9c9 | galberding | namespace amiro {
|
14 | e1f1c4b5 | galberding | |
15 | b8b3a9c9 | galberding | struct node {
|
16 | uint8_t id; |
||
17 | uint8_t flag; |
||
18 | uint8_t left; |
||
19 | uint8_t right; |
||
20 | a07a7a1c | galberding | uint8_t visited; |
21 | b8b3a9c9 | galberding | types::position pL; // Left
|
22 | types::position pR; // Right
|
||
23 | types::position pB; // Back
|
||
24 | }; |
||
25 | e1f1c4b5 | galberding | |
26 | b8b3a9c9 | galberding | class AmiroMap { |
27 | public:
|
||
28 | uint8_t get_next() { return next; }
|
||
29 | e1f1c4b5 | galberding | |
30 | b8b3a9c9 | galberding | void set_next( uint8_t next) { this->next = next; } |
31 | e1f1c4b5 | galberding | |
32 | b8b3a9c9 | galberding | uint8_t get_current() { return current; }
|
33 | |||
34 | void set_current( uint8_t current) { this->current = current; } |
||
35 | |||
36 | node* get_nodeList() { return nodeList; }
|
||
37 | |||
38 | uint8_t get_nodeCount() { return nodeCount; }
|
||
39 | |||
40 | void set_nodeCount( uint8_t nodeCount) { this->nodeCount = nodeCount; } |
||
41 | e1f1c4b5 | galberding | |
42 | b8b3a9c9 | galberding | bool get_valid() { return valid; } |
43 | |||
44 | void set_valid( bool valid) { this->valid = valid; } |
||
45 | |||
46 | AmiroMap(Global *global) : global{global} {} |
||
47 | |||
48 | /**
|
||
49 | * Initialize a new map from configuration.
|
||
50 | * @param config map configuration array
|
||
51 | *
|
||
52 | */
|
||
53 | d2230e6e | galberding | uint8_t initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]); |
54 | b8b3a9c9 | galberding | |
55 | |||
56 | /**
|
||
57 | Update the internal map state according to the detected fixpoint
|
||
58 | This function should be called for a generic update, each can cycle and in
|
||
59 | case a fixpoint on one side is detected.
|
||
60 | Internal state maschine will go into an error state in case both sensors are black.
|
||
61 | |||
62 | @param left fixpoint on left side detected
|
||
63 | @param right fixpoint on right side detected
|
||
64 | @param strategy current line follow strategy
|
||
65 | |||
66 | |||
67 | */
|
||
68 | void update(bool left, bool right, LineFollowStrategy strategy); |
||
69 | |||
70 | private:
|
||
71 | Global *global; |
||
72 | bool valid = false; |
||
73 | uint8_t nodeCount = 0;
|
||
74 | node nodeList[MAX_NODES]; |
||
75 | uint8_t current = 0;
|
||
76 | uint8_t next = 0;
|
||
77 | a07a7a1c | galberding | void visitNode(uint8_t id);
|
78 | }; |
||
79 | b8b3a9c9 | galberding | }; // namespace amiro
|
80 | e1f1c4b5 | galberding | |
81 | #endif /* AMIRO_MAP */ |