Revision 7520e117 devices/DiWheelDrive/amiro_map.hpp
| devices/DiWheelDrive/amiro_map.hpp | ||
|---|---|---|
| 5 | 5 |
#include "linefollow.hpp" |
| 6 | 6 |
#include <amiroosconf.h> |
| 7 | 7 |
#include <ch.hpp> |
| 8 |
#include <cstdint> |
|
| 9 |
|
|
| 10 |
#define MAX_NODES 10 |
|
| 11 |
#define NODE_ATTRIBUTES 3 |
|
| 12 | 8 |
|
| 13 | 9 |
namespace amiro {
|
| 14 | 10 |
|
| 15 |
struct node {
|
|
| 16 |
uint8_t id; |
|
| 17 |
uint8_t flag; |
|
| 18 |
uint8_t left; |
|
| 19 |
uint8_t right; |
|
| 20 |
uint8_t visited; |
|
| 21 |
types::position pL; // Left |
|
| 22 |
types::position pR; // Right |
|
| 23 |
types::position pB; // Back |
|
| 24 |
}; |
|
| 11 |
struct node {
|
|
| 12 |
uint8_t id; |
|
| 13 |
uint8_t flag; |
|
| 14 |
uint8_t left; |
|
| 15 |
uint8_t right; |
|
| 16 |
uint8_t visited; |
|
| 17 |
types::position pL; // Left |
|
| 18 |
types::position pR; // Right |
|
| 19 |
types::position pB; // Back |
|
| 20 |
}; |
|
| 21 |
|
|
| 22 |
struct map_state {
|
|
| 23 |
// 0 - left, 1- right |
|
| 24 |
uint8_t strategy; |
|
| 25 |
// Node ID of last detected fixpoint |
|
| 26 |
uint8_t current; |
|
| 27 |
// Next node ID |
|
| 28 |
uint8_t next; |
|
| 29 |
//Traveled Distance between current and next in % |
|
| 30 |
uint8_t dist; |
|
| 31 |
// True if the current loaded map is valid |
|
| 32 |
bool valid; |
|
| 33 |
}; |
|
| 25 | 34 |
|
| 26 | 35 |
class AmiroMap {
|
| 27 | 36 |
public: |
| 28 |
uint8_t get_next() { return next; }
|
|
| 29 |
|
|
| 30 |
void set_next( uint8_t next) { this->next = next; }
|
|
| 31 |
|
|
| 32 |
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 |
|
|
| 42 |
bool get_valid() { return valid; }
|
|
| 43 |
|
|
| 44 |
void set_valid( bool valid) { this->valid = valid; }
|
|
| 37 |
map_state * get_state() { return &state; }
|
|
| 45 | 38 |
|
| 46 | 39 |
AmiroMap(Global *global) : global{global} {}
|
| 47 | 40 |
|
| ... | ... | |
| 50 | 43 |
* @param config map configuration array |
| 51 | 44 |
* |
| 52 | 45 |
*/ |
| 53 |
uint8_t initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]);
|
|
| 46 |
uint8_t initialize();
|
|
| 54 | 47 |
|
| 55 | 48 |
|
| 56 | 49 |
/** |
| 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 |
|
|
| 50 |
* Update the internal map state according to the detected fixpoint
|
|
| 51 |
* This function should be called for a generic update, each can cycle and in
|
|
| 52 |
* case a fixpoint on one side is detected.
|
|
| 53 |
* Internal state maschine will go into an error state in case both sensors are black.
|
|
| 54 |
* |
|
| 55 |
* @param left fixpoint on left side detected
|
|
| 56 |
* @param right fixpoint on right side detected
|
|
| 57 |
* @param strategy current line follow strategy
|
|
| 58 |
* |
|
| 59 |
* |
|
| 67 | 60 |
*/ |
| 68 |
void update(bool left, bool right, LineFollowStrategy strategy);
|
|
| 61 |
void update(LineFollowStrategy strategy); |
|
| 69 | 62 |
|
| 70 | 63 |
private: |
| 71 | 64 |
Global *global; |
| 72 |
bool valid = false; |
|
| 65 |
LineFollowStrategy lfStrategy = LineFollowStrategy::EDGE_RIGHT; |
|
| 66 |
// Keeps track of the internal map state |
|
| 67 |
map_state state; |
|
| 68 |
// Holds the amount of detected node of the map |
|
| 73 | 69 |
uint8_t nodeCount = 0; |
| 70 |
// Stores all nodes and the corresponding coordinates of last detected fixpoints |
|
| 74 | 71 |
node nodeList[MAX_NODES]; |
| 75 |
uint8_t current = 0; |
|
| 76 |
uint8_t next = 0; |
|
| 72 |
// If driving over fixpoint prevent continuous updating |
|
| 73 |
// True if left sensor is driving over black |
|
| 74 |
bool leftDetected = false; |
|
| 75 |
// True if right sensor is driving over black |
|
| 76 |
bool rightDetected = false; |
|
| 77 |
|
|
| 78 |
/** |
|
| 79 |
* Recursively search through all nodes in the node list and |
|
| 80 |
* mark all reached nodes as visited. |
|
| 81 |
* |
|
| 82 |
* @param id node from where to start visiting the other nodes |
|
| 83 |
* |
|
| 84 |
*/ |
|
| 77 | 85 |
void visitNode(uint8_t id); |
| 78 | 86 |
}; |
| 79 | 87 |
}; // namespace amiro |
Also available in: Unified diff