amiro-os / devices / DiWheelDrive / amiro_map.cpp @ a3c54343
History | View | Annotate | Download (5.357 KB)
1 | e1f1c4b5 | galberding | #include "amiro_map.hpp" |
---|---|---|---|
2 | |||
3 | 7520e117 | galberding | uint8_t AmiroMap::initialize(){ |
4 | e1f1c4b5 | galberding | |
5 | b8b3a9c9 | galberding | // Clear old values in case the map is initialized again
|
6 | 7520e117 | galberding | this->state.current = 0; |
7 | this->state.next = 0; |
||
8 | this->state.valid = false; |
||
9 | 6acaea07 | galberding | this->nodeCount = 0; |
10 | bdac5bec | galberding | this->state.strategy = 0x1; |
11 | e1f1c4b5 | galberding | |
12 | b8b3a9c9 | galberding | // convert proto map to internal representation
|
13 | e1f1c4b5 | galberding | for (int i=0; i<MAX_NODES; i++){ |
14 | 7520e117 | galberding | if(global->testmap[i][2] == 0xff && i != 0){ |
15 | e1f1c4b5 | galberding | break;
|
16 | 7520e117 | galberding | } else if (global->testmap[i][2] == 0xff && i == 0) { |
17 | this->state.valid = false; |
||
18 | d2230e6e | galberding | return 255; |
19 | e1f1c4b5 | galberding | } |
20 | |||
21 | b8b3a9c9 | galberding | //look for start node (default is Node 0)
|
22 | 7520e117 | galberding | if (global->testmap[i][2] == 1 ) { |
23 | this->state.current = i;
|
||
24 | b8b3a9c9 | galberding | } |
25 | e1f1c4b5 | galberding | |
26 | b8b3a9c9 | galberding | this->nodeList[i].id = i;
|
27 | 7520e117 | galberding | this->nodeList[i].left = global->testmap[i][0]; |
28 | this->nodeList[i].right = global->testmap[i][1]; |
||
29 | this->nodeList[i].flag = global->testmap[i][2]; |
||
30 | b8b3a9c9 | galberding | this->nodeCount++;
|
31 | } |
||
32 | bdac5bec | galberding | this->state.next = this->nodeList[this->state.current].right; |
33 | e1f1c4b5 | galberding | |
34 | b8b3a9c9 | galberding | // TODO make validity check
|
35 | d2230e6e | galberding | |
36 | a07a7a1c | galberding | for (int j=0; j<nodeCount; j++) { |
37 | d2230e6e | galberding | this->nodeList[j].visited = 0; |
38 | a07a7a1c | galberding | visitNode(j); |
39 | for (int k = 0; k < nodeCount; k++) { |
||
40 | d2230e6e | galberding | if (this->nodeList[k].visited == 1) { |
41 | this->nodeList[k].visited = 0; |
||
42 | a07a7a1c | galberding | } else {
|
43 | 7520e117 | galberding | this->state.valid = false; |
44 | d2230e6e | galberding | return k;
|
45 | a07a7a1c | galberding | } |
46 | } |
||
47 | } |
||
48 | e1f1c4b5 | galberding | |
49 | 7520e117 | galberding | this->state.valid = true; |
50 | d2230e6e | galberding | return 42; |
51 | a07a7a1c | galberding | } |
52 | e1f1c4b5 | galberding | |
53 | a07a7a1c | galberding | void AmiroMap::visitNode(uint8_t id){
|
54 | d2230e6e | galberding | if (this->nodeList[id].visited == 1){ |
55 | a07a7a1c | galberding | return;
|
56 | }else{
|
||
57 | d2230e6e | galberding | nodeList[id].visited = 1;
|
58 | a07a7a1c | galberding | visitNode(this->nodeList[id].left);
|
59 | visitNode(this->nodeList[id].right);
|
||
60 | } |
||
61 | e1f1c4b5 | galberding | } |
62 | |||
63 | bdac5bec | galberding | 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
|
||
67 | 7520e117 | galberding | |
68 | // set the strategy directly, actually there is no need to store that variable in the class
|
||
69 | // but we will go with it for now to initialize everything properly.
|
||
70 | bdac5bec | galberding | uint8_t flag = 0;
|
71 | 7520e117 | galberding | this->lfStrategy = strategy;
|
72 | bdac5bec | galberding | // uint16_t WL = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
|
73 | // uint16_t WR = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
|
||
74 | 7520e117 | galberding | |
75 | // Check the wheel sensors
|
||
76 | bool left = global->linePID.BThresh >= WL;
|
||
77 | bool right = global->linePID.BThresh >= WR;
|
||
78 | types::position currentPos = global->odometry.getPosition(); |
||
79 | |||
80 | if (left && right) {
|
||
81 | // TODO A dangerous case -> amiro could be lifted
|
||
82 | bdac5bec | galberding | flag |= 255;
|
83 | 7520e117 | galberding | } |
84 | else if (left && !leftDetected) { |
||
85 | // The sensor on the left side of the Amiro is driving on black
|
||
86 | // To prevent continous fixpoint detection a point needs to be marked as currently detected
|
||
87 | // and released.
|
||
88 | leftDetected = true;
|
||
89 | nodeList[state.next].pR.f_x = currentPos.f_x; |
||
90 | nodeList[state.next].pR.f_y = currentPos.f_y; |
||
91 | nodeList[state.next].pR.f_z = currentPos.f_z; |
||
92 | nodeList[state.next].pR.x = currentPos.x; |
||
93 | nodeList[state.next].pR.y = currentPos.y; |
||
94 | nodeList[state.next].pR.z = currentPos.z; |
||
95 | nodeList[state.next].visited |= 0x01;
|
||
96 | state.current = state.next; |
||
97 | state.next = nodeList[state.current].right; |
||
98 | state.strategy = 0x01;
|
||
99 | bdac5bec | galberding | state.eLength = 0; // Reset length to get recalculated after fixpoint |
100 | flag |= 0x1;
|
||
101 | 7520e117 | galberding | } |
102 | else if (right && !rightDetected) { |
||
103 | // Same as left only for the right sensor.
|
||
104 | rightDetected = true;
|
||
105 | nodeList[state.next].pL.f_x = currentPos.f_x; |
||
106 | nodeList[state.next].pL.f_y = currentPos.f_y; |
||
107 | nodeList[state.next].pL.f_z = currentPos.f_z; |
||
108 | nodeList[state.next].pL.x = currentPos.x; |
||
109 | nodeList[state.next].pL.y = currentPos.y; |
||
110 | nodeList[state.next].pL.z = currentPos.z; |
||
111 | nodeList[state.next].visited |= 0x02;
|
||
112 | state.current = state.next; |
||
113 | state.next = nodeList[state.current].left; |
||
114 | bdac5bec | galberding | state.strategy = 0x2;
|
115 | state.eLength = 0; // Reset length to get recalculated after fixpoint |
||
116 | flag |= 0x2;
|
||
117 | 7520e117 | galberding | } |
118 | else if (!left && !right) { |
||
119 | // in case the fixpoint is not detected anymore
|
||
120 | leftDetected = false;
|
||
121 | rightDetected = false;
|
||
122 | bdac5bec | galberding | flag |= 0x4;
|
123 | 7520e117 | galberding | } |
124 | |||
125 | a07a7a1c | galberding | |
126 | 7520e117 | galberding | // update internal map_state
|
127 | // Update travel distance
|
||
128 | // check if the nodes of the specific strategy where visited
|
||
129 | bdac5bec | galberding | if (state.strategy
|
130 | a3c54343 | galberding | == nodeList[state.current].visited) { |
131 | bdac5bec | galberding | 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 | a3c54343 | galberding | // only calculate edge length if the node is already vivited
|
137 | if ((state.eLength == 0) && (state.strategy == nodeList[state.current].visited)) { |
||
138 | bdac5bec | galberding | state.eLength = calculateDist(&nodeList[state.next].pR, |
139 | &nodeList[state.current].pR); |
||
140 | } |
||
141 | a3c54343 | galberding | state.dist = calculateDist(&nodeList[state.current].pR, ¤tPos); |
142 | bdac5bec | galberding | } else {
|
143 | // Driving on the left edge
|
||
144 | a3c54343 | galberding | if ((state.eLength == 0) && |
145 | (state.strategy == nodeList[state.current].visited)) { |
||
146 | state.eLength = calculateDist(&nodeList[state.next].pR, |
||
147 | &nodeList[state.current].pR); |
||
148 | bdac5bec | galberding | } |
149 | a3c54343 | galberding | state.dist = calculateDist(&nodeList[state.current].pL, ¤tPos); |
150 | bdac5bec | galberding | |
151 | } |
||
152 | } |
||
153 | return flag;
|
||
154 | } |
||
155 | |||
156 | uint32_t AmiroMap::calculateDist(types::position *p1, types::position *p2) { |
||
157 | d02c536e | galberding | return (uint32_t) sqrt(pow((p2->x - p1->x)/10000, 2) + |
158 | pow((p2->y - p1->y)/10000, 2)); |
||
159 | e1f1c4b5 | galberding | } |
160 | a3c54343 | galberding | |
161 | uint8_t trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy, ut_states state){ |
||
162 | |||
163 | } |