Revision ec052975

View differences:

devices/DiWheelDrive/amiro_map.cpp
86 86
    // To prevent continous fixpoint detection a point needs to be marked as currently detected
87 87
    // and released.
88 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;
89
    copyPoint(&currentPos, &nodeList[state.next].pR);
95 90
    nodeList[state.next].visited |= 0x01;
96 91
    state.current = state.next;
97 92
    state.next = nodeList[state.current].right;
......
102 97
  else if (right && !rightDetected) {
103 98
    // Same as left only for the right sensor.
104 99
    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;
100
    copyPoint(&currentPos, &nodeList[state.next].pR);
111 101
    nodeList[state.next].visited |= 0x02;
112 102
    state.current = state.next;
113 103
    state.next = nodeList[state.current].left;
......
158 148
                          pow((p2->y - p1->y)/10000, 2));
159 149
}
160 150

  
161
uint8_t trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy, ut_states state){
151
uint8_t AmiroMap::trackUpdate(uint16_t WL, uint16_t WR, LineFollowStrategy strategy,
152
                    ut_states ut_state) {
153
  // Check if map is valid
154
  if (this->state.valid){
155
    return update(WL, WR, strategy);
156
  }
157

  
158
  // Create init node if none is there
159
  // We will not assign a point to the initial fixpoint because it is not clear if
160
  // start position is at the correct point
161
  if (nodeCount == 0) {
162
    createInitNode();
163
  }
164

  
165
  bool left = global->linePID.BThresh >= WL;
166
  bool right = global->linePID.BThresh >= WR;
167
  types::position currentPos = global->odometry.getPosition();
168

  
169
  // Assign fixpoint if side sensor is black
170
  // Do not update if update was already applied the round before (leftDetected || rightDetected) == true
171
  if ((left || right) && !(leftDetected || rightDetected)) {
172
    // Determine what strategy to use
173
    // assignFxp() will use strategy to assign the next point
174
    state.strategy = right ? 1 : 2;
175

  
176
    // Check if next point is reachable
177
    if (state.next == 255){
178
      // Prepare state values for switch
179
      assignFxp(&currentPos);
180
    }
181

  
182
  }else if (!(left || right)) {
183
    // TODO: do we need both?
184
    leftDetected = rightDetected = true;
185
}
186

  
187

  
188
}
189

  
190

  
191

  
192
void AmiroMap::calTravelState() {
193

  
194
}
195

  
196
void AmiroMap::checkMap() {
197

  
198
}
199

  
200

  
201
void AmiroMap::switchToNext(types::position *p1) {
202

  
203
  // Update point if node was not visited before
204
  if ((nodeList[state.next].visited & state.strategy) == 0){
205
      copyPoint(p1, &nodeList[state.next].p.arr[state.strategy - 1]);
206
      nodeList[state.next].visited |= state.strategy;
207
  }
208

  
209
  leftDetected = true;
210
  state.current = state.next;
211
  state.next = nodeList[state.current].edge.arr[state.strategy - 1];
212
  state.eLength = 0; // Reset length to get recalculated after fixpoint
213
  return;
214
}
215

  
216

  
217
void AmiroMap::copyPoint(types::position* from, types::position* to) {
218
  to->x = from->x;
219
  to->y = from->y;
220
  to->f_x = from->f_x;
221
}
222

  
223

  
224
void AmiroMap::createInitNode() {
225
  this->nodeCount = 0;
226
  state.current = addNode(255, 255, 1);
227
  state.next = 255;
228
}
229

  
230
uint8_t AmiroMap::getNearest(types::position *p1) {
231
  uint8_t b;
232
  return b;
233
}
234

  
235
uint8_t AmiroMap::assignFxp(types::position *p1) {
236

  
237
  // Magic happens to determine if fixpoint is close enough
238
  uint8_t id = getNearest(p1);
239
  if(id < 255){
240
    // strategy is either 1 - right or 2 - left
241
    // Copy current point to either left or right point
242
    copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);
243
    }else {
244
    // A new fixpoint needs to be created
245
    id = addNode(255, 255, 0);
246
    copyPoint(p1, &nodeList[id].p.arr[state.strategy - 1]);
247
  }
162 248

  
249
  // Prepare values for switching
250
  state.next = id;
251
  nodeList[state.current].edge.arr[state.strategy - 1] = id;
252
  // Mark point as visited
253
  nodeList[id].visited |= state.strategy;
254
  return id;
163 255
}

Also available in: Unified diff