Revision bdac5bec
| devices/DiWheelDrive/amiro_map.cpp | ||
|---|---|---|
| 1 | 1 |
#include "amiro_map.hpp" |
| 2 | 2 |
|
| 3 |
// #include <cstdint> |
|
| 4 |
// #include <math.h> |
|
| 5 |
|
|
| 6 |
|
|
| 7 | 3 |
uint8_t AmiroMap::initialize(){
|
| 8 | 4 |
|
| 9 | 5 |
// Clear old values in case the map is initialized again |
| ... | ... | |
| 11 | 7 |
this->state.next = 0; |
| 12 | 8 |
this->state.valid = false; |
| 13 | 9 |
this->nodeCount = 0; |
| 10 |
this->state.strategy = 0x1; |
|
| 14 | 11 |
|
| 15 | 12 |
// convert proto map to internal representation |
| 16 | 13 |
for (int i=0; i<MAX_NODES; i++){
|
| ... | ... | |
| 32 | 29 |
this->nodeList[i].flag = global->testmap[i][2]; |
| 33 | 30 |
this->nodeCount++; |
| 34 | 31 |
} |
| 32 |
this->state.next = this->nodeList[this->state.current].right; |
|
| 35 | 33 |
|
| 36 | 34 |
// TODO make validity check |
| 37 | 35 |
|
| ... | ... | |
| 62 | 60 |
} |
| 63 | 61 |
} |
| 64 | 62 |
|
| 65 |
void AmiroMap::update(LineFollowStrategy strategy) {
|
|
| 66 |
// Each time at the end of the user thread state maschine, when the strategy |
|
| 67 |
// is changed or a fixpoint is detected, this method gets called. |
|
| 68 |
// The strategy change only has an effect on the fixpoint or in particular if |
|
| 69 |
// a fixpoint is detected. |
|
| 63 |
uint8_t AmiroMap::update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy) {
|
|
| 64 |
// Called each time at the end of the user thread state machine |
|
| 65 |
// The bottom sensors will be checked for black ground which is interpreted as |
|
| 66 |
// filxpoint |
|
| 70 | 67 |
|
| 71 | 68 |
// set the strategy directly, actually there is no need to store that variable in the class |
| 72 | 69 |
// but we will go with it for now to initialize everything properly. |
| 70 |
uint8_t flag = 0; |
|
| 73 | 71 |
this->lfStrategy = strategy; |
| 74 |
uint16_t WL = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset(); |
|
| 75 |
uint16_t WR = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset(); |
|
| 72 |
// uint16_t WL = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
|
|
| 73 |
// uint16_t WR = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
|
|
| 76 | 74 |
|
| 77 | 75 |
// Check the wheel sensors |
| 78 | 76 |
bool left = global->linePID.BThresh >= WL; |
| ... | ... | |
| 81 | 79 |
|
| 82 | 80 |
if (left && right) {
|
| 83 | 81 |
// TODO A dangerous case -> amiro could be lifted |
| 82 |
flag |= 255; |
|
| 84 | 83 |
} |
| 85 | 84 |
else if (left && !leftDetected) {
|
| 86 | 85 |
// The sensor on the left side of the Amiro is driving on black |
| ... | ... | |
| 97 | 96 |
state.current = state.next; |
| 98 | 97 |
state.next = nodeList[state.current].right; |
| 99 | 98 |
state.strategy = 0x01; |
| 99 |
state.eLength = 0; // Reset length to get recalculated after fixpoint |
|
| 100 |
flag |= 0x1; |
|
| 100 | 101 |
} |
| 101 | 102 |
else if (right && !rightDetected) {
|
| 102 | 103 |
// Same as left only for the right sensor. |
| ... | ... | |
| 110 | 111 |
nodeList[state.next].visited |= 0x02; |
| 111 | 112 |
state.current = state.next; |
| 112 | 113 |
state.next = nodeList[state.current].left; |
| 113 |
state.strategy = 0x02; |
|
| 114 |
state.strategy = 0x2; |
|
| 115 |
state.eLength = 0; // Reset length to get recalculated after fixpoint |
|
| 116 |
flag |= 0x2; |
|
| 114 | 117 |
} |
| 115 | 118 |
else if (!left && !right) {
|
| 116 | 119 |
// in case the fixpoint is not detected anymore |
| 117 | 120 |
leftDetected = false; |
| 118 | 121 |
rightDetected = false; |
| 122 |
flag |= 0x4; |
|
| 119 | 123 |
} |
| 120 | 124 |
|
| 121 | 125 |
|
| 122 | 126 |
// update internal map_state |
| 123 | 127 |
// Update travel distance |
| 124 | 128 |
// check if the nodes of the specific strategy where visited |
| 125 |
// if (state.strategy |
|
| 126 |
// == (nodeList[state.current].visited & nodeList[state.next].visited)) {
|
|
| 127 |
// // only update distance if both nodes were visited |
|
| 128 |
// if (state.strategy == 0x01) {
|
|
| 129 |
// // Amiro is driving on the right edge |
|
| 130 |
// state.dist = (uint8_t) sqrt( |
|
| 131 |
// pow(nodeList[state.next].pR.x - nodeList[state.current].pR.x, 2) |
|
| 132 |
// + pow(nodeList[state.next].pR.y - nodeList[state.current].pR.y, 2)); |
|
| 133 |
// } else {
|
|
| 134 |
// // Driving on the left edge |
|
| 135 |
|
|
| 136 |
// } |
|
| 137 |
// } |
|
| 129 |
if (state.strategy |
|
| 130 |
== (nodeList[state.current].visited & nodeList[state.next].visited)) {
|
|
| 131 |
flag |= 0x8; |
|
| 132 |
// only update distance if both nodes were visited |
|
| 133 |
// Calculate estimated length of the edge |
|
| 134 |
if (state.strategy == 0x01) {
|
|
| 135 |
// Amiro is driving on the right edge |
|
| 136 |
if (state.eLength == 0) {
|
|
| 137 |
state.eLength = calculateDist(&nodeList[state.next].pR, |
|
| 138 |
&nodeList[state.current].pR); |
|
| 139 |
} |
|
| 140 |
state.dist = calculateDist(&nodeList[state.next].pR, ¤tPos) * 100 / |
|
| 141 |
state.eLength; |
|
| 142 |
} else {
|
|
| 143 |
// Driving on the left edge |
|
| 144 |
if (state.eLength == 0) {
|
|
| 145 |
state.eLength = |
|
| 146 |
calculateDist(&nodeList[state.next].pL, &nodeList[state.current].pL); |
|
| 147 |
} |
|
| 148 |
state.dist = calculateDist(&nodeList[state.next].pL, ¤tPos) * 100 / state.eLength; |
|
| 149 |
|
|
| 150 |
} |
|
| 151 |
} |
|
| 152 |
return flag; |
|
| 153 |
} |
|
| 154 |
|
|
| 155 |
uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) {
|
|
| 156 |
return (uint32_t) sqrt(pow(p2->x - p1->x, 2) + |
|
| 157 |
pow(p2->y - p1->y, 2)); |
|
| 138 | 158 |
} |
| devices/DiWheelDrive/amiro_map.hpp | ||
|---|---|---|
| 6 | 6 |
#include <amiroosconf.h> |
| 7 | 7 |
#include <ch.hpp> |
| 8 | 8 |
|
| 9 |
|
|
| 9 | 10 |
namespace amiro {
|
| 10 | 11 |
|
| 11 | 12 |
struct node {
|
| ... | ... | |
| 27 | 28 |
// Next node ID |
| 28 | 29 |
uint8_t next; |
| 29 | 30 |
//Traveled Distance between current and next in % |
| 30 |
uint8_t dist;
|
|
| 31 |
uint32_t dist;
|
|
| 31 | 32 |
// True if the current loaded map is valid |
| 32 | 33 |
bool valid; |
| 34 |
// Length of the currently traveled edge |
|
| 35 |
uint32_t eLength; |
|
| 33 | 36 |
}; |
| 34 | 37 |
|
| 35 | 38 |
class AmiroMap {
|
| ... | ... | |
| 58 | 61 |
* |
| 59 | 62 |
* |
| 60 | 63 |
*/ |
| 61 |
void update(LineFollowStrategy strategy);
|
|
| 64 |
uint8_t update(uint16_t WL, uint16_t WR, LineFollowStrategy strategy);
|
|
| 62 | 65 |
|
| 63 | 66 |
private: |
| 64 | 67 |
Global *global; |
| ... | ... | |
| 83 | 86 |
* |
| 84 | 87 |
*/ |
| 85 | 88 |
void visitNode(uint8_t id); |
| 89 |
|
|
| 90 |
/** |
|
| 91 |
* Calculate the distance between two given points |
|
| 92 |
* |
|
| 93 |
* @param p1 point 1 |
|
| 94 |
* @param p2 point 2 |
|
| 95 |
* |
|
| 96 |
*/ |
|
| 97 |
uint32_t calculateDist(types::position *p1, types::position *p2); |
|
| 86 | 98 |
}; |
| 87 | 99 |
}; // namespace amiro |
| 88 | 100 |
|
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 785 | 785 |
// Test suit for amiro map |
| 786 | 786 |
|
| 787 | 787 |
|
| 788 |
setAttributes(global.testmap, 0, 1, 2, 1); |
|
| 789 |
setAttributes(global.testmap, 1, 2, 2, 0); |
|
| 790 |
setAttributes(global.testmap, 2, 1, 0, 0); |
|
| 791 |
setAttributes(global.testmap, 3, 0, 0, 0xff); |
|
| 788 |
|
|
| 792 | 789 |
// AmiroMap map = AmiroMap(&global); |
| 793 | 790 |
|
| 794 | 791 |
// -------------------------------------------------- |
| 795 | 792 |
|
| 796 | 793 |
global.tcase = 0; |
| 797 |
map.initialize(); |
|
| 794 |
// Set basic valid map configuration |
|
| 795 |
setAttributes(global.testmap, 0, 1, 2, 1); |
|
| 796 |
setAttributes(global.testmap, 1, 2, 2, 0); |
|
| 797 |
setAttributes(global.testmap, 2, 1, 0, 0); |
|
| 798 |
setAttributes(global.testmap, 3, 0, 0, 0xff); |
|
| 799 |
chprintf((BaseSequentialStream *)&global.sercanmux1, "Init Case: %d, res: %d\n",global.tcase, map.initialize()); |
|
| 798 | 800 |
global.testres[global.tcase] = map.get_state()->valid; |
| 799 | 801 |
|
| 800 |
global.tcase++; // 1 |
|
| 802 |
global.tcase++; // 1 |
|
| 803 |
// Test map fail if first node is flagged with end |
|
| 801 | 804 |
setAttributes(global.testmap, 0, 1, 2, 0xff); |
| 802 | 805 |
map.initialize(); |
| 803 | 806 |
global.testres[global.tcase] = !map.get_state()->valid; |
| 804 | 807 |
|
| 805 | 808 |
global.tcase++; // 2 |
| 809 |
// Test if node 2 is set as start node |
|
| 806 | 810 |
setAttributes(global.testmap, 0, 1, 2, 0); |
| 807 | 811 |
setAttributes(global.testmap, 2, 1, 0, 1); |
| 808 | 812 |
map.initialize(); |
| 809 | 813 |
global.testres[global.tcase] = map.get_state()->current == 2; |
| 810 | 814 |
|
| 811 | 815 |
global.tcase++; // 3 |
| 816 |
// Test if non reachable nodes will trigger invalid map |
|
| 812 | 817 |
setAttributes(global.testmap, 3, 0, 0, 0); |
| 813 | 818 |
setAttributes(global.testmap, 4, 0, 0, 0xff); |
| 814 | 819 |
map.initialize(); |
| 815 | 820 |
global.testres[global.tcase] = !map.get_state()->valid; |
| 816 | 821 |
|
| 822 |
global.tcase++; // 4 |
|
| 823 |
// Test Reinitialization |
|
| 824 |
setAttributes(global.testmap, 0, 1, 2, 1); |
|
| 825 |
setAttributes(global.testmap, 1, 2, 2, 0); |
|
| 826 |
setAttributes(global.testmap, 2, 1, 0, 0); |
|
| 827 |
setAttributes(global.testmap, 3, 0, 0, 0xff); |
|
| 828 |
map.initialize(); |
|
| 829 |
global.testres[global.tcase] = map.get_state()->valid; |
|
| 830 |
|
|
| 831 |
global.odometry.resetPosition(); |
|
| 832 |
uint8_t ret = 0; |
|
| 833 |
global.tcase++; // 5 |
|
| 834 |
// Test update under normal linefollowing without fixpoint |
|
| 835 |
ret = map.update(20000, 20000, LineFollowStrategy::EDGE_RIGHT); |
|
| 836 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
|
| 837 |
"Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret, |
|
| 838 |
map.get_state()->current, map.get_state()->next); |
|
| 839 |
// No case should be true because neither was a node visited nor |
|
| 840 |
// was a fixpoint detected. |
|
| 841 |
global.testres[global.tcase] = (ret == 0x4); |
|
| 842 |
|
|
| 843 |
|
|
| 844 |
global.odometry.setPosition(1.0, 0.0, 0.0); |
|
| 845 |
global.tcase++; // 6 |
|
| 846 |
// Fixpoint on left side |
|
| 847 |
ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT); |
|
| 848 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
|
| 849 |
"Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret, |
|
| 850 |
map.get_state()->current, map.get_state()->next); |
|
| 851 |
// No case should be true because neither was a node visited nor |
|
| 852 |
// was a fixpoint detected. |
|
| 853 |
// global.odometry |
|
| 854 |
global.testres[global.tcase] = (ret == 0x1) |
|
| 855 |
&& (map.get_state()->strategy == 0x01) |
|
| 856 |
&& (map.get_state()->dist == 0) |
|
| 857 |
&& (map.get_state()->current == 2); |
|
| 858 |
|
|
| 859 |
|
|
| 860 |
global.odometry.setPosition(1.5, 0.0, 0.0); |
|
| 861 |
global.tcase++; // 7 |
|
| 862 |
// Fixpoint on left side, no update should appear because fixpoint already |
|
| 863 |
// marked |
|
| 864 |
ret = map.update(0, 20000, LineFollowStrategy::EDGE_RIGHT); |
|
| 865 |
chprintf((BaseSequentialStream *)&global.sercanmux1, |
|
| 866 |
"Update test %d: Ret %d, cur %d, nex %d\n", global.tcase, ret, |
|
| 867 |
map.get_state()->current, map.get_state()->next); |
|
| 868 |
// No case should be true because neither was a node visited nor |
|
| 869 |
// was a fixpoint detected. |
|
| 870 |
global.testres[global.tcase] = (ret == 0x00) |
|
| 871 |
&& (map.get_state()->strategy == 0x01); |
|
| 872 |
// && (map.get_state()->dist == 0); |
|
| 873 |
|
|
| 874 |
|
|
| 875 |
|
|
| 876 |
|
|
| 877 |
|
|
| 817 | 878 |
int failed = 0; |
| 818 | 879 |
int passed = 0; |
| 819 | 880 |
for (int i = 0; i <= global.tcase; i++) {
|
Also available in: Unified diff