Revision bdac5bec devices/DiWheelDrive/amiro_map.cpp
| 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 |
} |
Also available in: Unified diff