Revision 4d54a507
| devices/DiWheelDrive/amiro_map.cpp | ||
|---|---|---|
| 20 | 20 |
return 255; |
| 21 | 21 |
} |
| 22 | 22 |
|
| 23 |
//look for start node (default is Node 0) |
|
| 24 |
if (global->testmap[i][2] == 1 ) {
|
|
| 25 |
this->state.current = i; |
|
| 26 |
} |
|
| 23 |
addNode(global->testmap[i][0], global->testmap[i][1], |
|
| 24 |
global->testmap[i][2]); |
|
| 27 | 25 |
|
| 28 |
this->nodeList[i].id = i; |
|
| 29 |
this->nodeList[i].edge.edge_id.left = global->testmap[i][0]; |
|
| 30 |
this->nodeList[i].edge.edge_id.right = global->testmap[i][1]; |
|
| 31 |
this->nodeList[i].flag = global->testmap[i][2]; |
|
| 32 |
this->nodeCount++; |
|
| 26 |
// look for start node (default is Node 0) |
|
| 27 |
if (global->testmap[i][2] == 1) {
|
|
| 28 |
this->state.current = this->nodeCount; |
|
| 29 |
} |
|
| 33 | 30 |
} |
| 34 |
this->state.next = this->nodeList[this->state.current].edge.edge_id.right; |
|
| 31 |
// Set the next node according to the current strategy |
|
| 32 |
this->state.next = this->nodeList[this->state.current].edge.arr[state.strategy-1]; |
|
| 35 | 33 |
|
| 36 | 34 |
// TODO make validity check |
| 37 |
|
|
| 38 | 35 |
for (int j=0; j<nodeCount; j++) {
|
| 39 | 36 |
this->nodeList[j].visited = 0; |
| 40 | 37 |
visitNode(j); |
| ... | ... | |
| 209 | 206 |
state.next = 255; |
| 210 | 207 |
} |
| 211 | 208 |
|
| 209 |
uint8_t AmiroMap::addNode(uint8_t l, uint8_t r, uint8_t flags){
|
|
| 210 |
uint8_t id = nodeCount; |
|
| 211 |
nodeCount++; |
|
| 212 |
nodeList[id].edge.edge_id.left = l; |
|
| 213 |
nodeList[id].edge.edge_id.right = r; |
|
| 214 |
nodeList[id].flag = flags; |
|
| 215 |
return id; |
|
| 216 |
} |
|
| 212 | 217 |
|
| 213 |
uint8_t AmiroMap::getNearest(types::position *p1) {
|
|
| 218 |
uint8_t AmiroMap::getNearest(types::position *p1) {
|
|
| 214 | 219 |
|
| 215 | 220 |
uint8_t actualStrategy = this->lfStrategy == EDGE_LEFT ? 1 : 2; |
| 216 | 221 |
uint32_t thresh = global->nodeDistThresh; // TODO: find good thresh value in cm |
| ... | ... | |
| 278 | 283 |
this->nodeCount = 0; |
| 279 | 284 |
this->state.current = 0; |
| 280 | 285 |
this->state.next= 0; |
| 281 |
|
|
| 286 |
this->state.valid = false; |
|
| 287 |
this->state.strategy = this->lfStrategy == EDGE_LEFT ? 1 : 2; |
|
| 282 | 288 |
} |
| devices/DiWheelDrive/amiro_map.hpp | ||
|---|---|---|
| 37 | 37 |
node * getNodeList(){return nodeList; }
|
| 38 | 38 |
|
| 39 | 39 |
|
| 40 |
AmiroMap(Global *global) : global{global} {}
|
|
| 40 |
AmiroMap(Global *global, LineFollowStrategy lfStrategy) : global(global), lfStrategy(lfStrategy) {}
|
|
| 41 | 41 |
|
| 42 | 42 |
/** |
| 43 | 43 |
* Initialize a new map from configuration. |
| ... | ... | |
| 46 | 46 |
*/ |
| 47 | 47 |
uint8_t initialize(); |
| 48 | 48 |
|
| 49 |
void reset(); |
|
| 49 | 50 |
|
| 50 | 51 |
/** |
| 51 | 52 |
* Update the internal map state according to the detected fixpoint |
| ... | ... | |
| 127 | 128 |
|
| 128 | 129 |
// Either create new fxpoint or assign point to existing one |
| 129 | 130 |
uint8_t assignFxp(types::position *p1); |
| 130 |
void reset(); |
|
| 131 |
|
|
| 131 | 132 |
}; |
| 132 | 133 |
|
| 133 | 134 |
}; // namespace amiro |
| devices/DiWheelDrive/global.hpp | ||
|---|---|---|
| 1 | 1 |
#ifndef AMIRO_GLOBAL_HPP_ |
| 2 | 2 |
#define AMIRO_GLOBAL_HPP_ |
| 3 | 3 |
|
| 4 |
#include <cstdint> |
|
| 4 |
// #include <cstdint>
|
|
| 5 | 5 |
#include <hal.h> |
| 6 | 6 |
#include <qei.h> |
| 7 | 7 |
|
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 1 | 1 |
#include "userthread.hpp" |
| 2 |
#include "amiro/Constants.h" |
|
| 3 | 2 |
#include "amiro_map.hpp" |
| 4 | 3 |
#include "global.hpp" |
| 5 | 4 |
#include "linefollow.hpp" |
| ... | ... | |
| 284 | 283 |
running = false; |
| 285 | 284 |
LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT; |
| 286 | 285 |
LineFollow lf(&global); |
| 287 |
AmiroMap map(&global); |
|
| 286 |
AmiroMap map(&global, lStrategy);
|
|
| 288 | 287 |
/* |
| 289 | 288 |
* LOOP |
| 290 | 289 |
*/ |
| ... | ... | |
| 808 | 807 |
"ID: %d, l: %d, r: %d \n", map.getNodeList()[i].id, |
| 809 | 808 |
map.getNodeList()[i].left, map.getNodeList()[i].right); |
| 810 | 809 |
} |
| 811 |
newState = UT_IDLE; |
|
| 810 |
// Start test suit for trackUpdate |
|
| 811 |
map.reset(); |
|
| 812 |
global.tcase = 0; |
|
| 813 |
newState = UT_TEST_MAP_STATE; |
|
| 812 | 814 |
break; |
| 815 |
// -------------------------------------------------- |
|
| 813 | 816 |
case ut_states::UT_TEST_MAP_STATE:{
|
| 814 | 817 |
// Test suit for amiro map |
| 815 | 818 |
|
| ... | ... | |
| 819 | 822 |
|
| 820 | 823 |
// -------------------------------------------------- |
| 821 | 824 |
|
| 822 |
global.tcase = 0;
|
|
| 825 |
global.tcase++;
|
|
| 823 | 826 |
// Set basic valid map configuration |
| 824 | 827 |
setAttributes(global.testmap, 0, 1, 2, 1); |
| 825 | 828 |
setAttributes(global.testmap, 1, 2, 2, 0); |
Also available in: Unified diff