Revision 7520e117

View differences:

devices/DiWheelDrive/amiro_map.cpp
1 1
#include "amiro_map.hpp"
2
#include <cstdint>
3 2

  
3
// #include <cstdint>
4
// #include <math.h>
4 5

  
5
uint8_t AmiroMap::initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]){
6

  
7
uint8_t AmiroMap::initialize(){
6 8

  
7 9
  // Clear old values in case the map is initialized again
8
  uint8_t visitTable[MAX_NODES];
9
  this->current = 0;
10
  this->next = 0;
11
  this->valid = false;
10
  this->state.current = 0;
11
  this->state.next = 0;
12
  this->state.valid = false;
12 13

  
13 14
  // convert proto map to internal representation
14 15
  for (int i=0; i<MAX_NODES; i++){
15
    if(config[i][2] == 0xff && i != 0){
16
    if(global->testmap[i][2] == 0xff && i != 0){
16 17
      break;
17
    } else if (config[i][2] == 0xff && i == 0) {
18
      this->valid = false;
18
    } else if (global->testmap[i][2] == 0xff && i == 0) {
19
      this->state.valid = false;
19 20
      return 255;
20 21
    }
21 22

  
22 23
    //look for start node (default is Node 0)
23
    if (config[i][2] == 1 ) {
24
      this->current= i;
24
    if (global->testmap[i][2] == 1 ) {
25
      this->state.current = i;
25 26
    }
26 27

  
27 28
    this->nodeList[i].id = i;
28
    this->nodeList[i].left = config[i][0];
29
    this->nodeList[i].right = config[i][1];
30
    this->nodeList[i].flag = config[i][2];
29
    this->nodeList[i].left = global->testmap[i][0];
30
    this->nodeList[i].right = global->testmap[i][1];
31
    this->nodeList[i].flag = global->testmap[i][2];
31 32
    this->nodeCount++;
32 33
  }
33 34

  
......
40 41
      if (this->nodeList[k].visited == 1) {
41 42
        this->nodeList[k].visited = 0;
42 43
      } else {
43
        this->valid = false;
44
        this->state.valid = false;
44 45
        return k;
45 46
      }
46 47
    }
47 48
  }
48 49

  
49
  this->valid = true;
50
  this->state.valid = true;
50 51
  return 42;
51 52
}
52 53

  
......
60 61
  }
61 62
}
62 63

  
64
void AmiroMap::update(LineFollowStrategy strategy) {
65
  // Each time at the end of the user thread state maschine, when the strategy
66
  // is changed or a fixpoint is detected, this method gets called.
67
  // The strategy change only has an effect on the fixpoint or in particular if
68
  // a fixpoint is detected.
69

  
70
  // set the strategy directly, actually there is no need to store that variable in the class
71
  // but we will go with it for now to initialize everything properly.
72
  this->lfStrategy = strategy;
73
  uint16_t WL = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_LEFT].getProximityScaledWoOffset();
74
  uint16_t WR = global->vcnl4020[constants::DiWheelDrive::PROX_WHEEL_RIGHT].getProximityScaledWoOffset();
75

  
76
  // Check the wheel sensors
77
  bool left = global->linePID.BThresh >= WL;
78
  bool right = global->linePID.BThresh >= WR;
79
  types::position currentPos = global->odometry.getPosition();
80

  
81
  if (left && right) {
82
    // TODO A dangerous case -> amiro could be lifted
83
  }
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
  }
100
  else if (right && !rightDetected) {
101
    // Same as left only for the right sensor.
102
    rightDetected = true;
103
    nodeList[state.next].pL.f_x = currentPos.f_x;
104
    nodeList[state.next].pL.f_y = currentPos.f_y;
105
    nodeList[state.next].pL.f_z = currentPos.f_z;
106
    nodeList[state.next].pL.x = currentPos.x;
107
    nodeList[state.next].pL.y = currentPos.y;
108
    nodeList[state.next].pL.z = currentPos.z;
109
    nodeList[state.next].visited |= 0x02;
110
    state.current = state.next;
111
    state.next = nodeList[state.current].left;
112
    state.strategy = 0x02;
113
  }
114
  else if (!left && !right) {
115
    // in case the fixpoint is not detected anymore
116
    leftDetected = false;
117
    rightDetected = false;
118
  }
119

  
63 120

  
64
void AmiroMap::update(bool left, bool right, LineFollowStrategy strategy) {
121
  // update internal map_state
122
  // Update travel distance
123
  // check if the nodes of the specific strategy where visited
124
  // if (state.strategy
125
  //     == (nodeList[state.current].visited & nodeList[state.next].visited)) {
126
  //   // only update distance if both nodes were visited
127
  //   if (state.strategy == 0x01) {
128
  //     // Amiro is driving on the right edge
129
  //     state.dist = (uint8_t) sqrt(
130
  //                                 pow(nodeList[state.next].pR.x - nodeList[state.current].pR.x, 2)
131
  //                                 + pow(nodeList[state.next].pR.y - nodeList[state.current].pR.y, 2));
132
  //   } else {
133
  //     // Driving on the left edge
65 134

  
135
  //   }
136
  // }
66 137
}
devices/DiWheelDrive/amiro_map.hpp
5 5
#include "linefollow.hpp"
6 6
#include <amiroosconf.h>
7 7
#include <ch.hpp>
8
#include <cstdint>
9

  
10
#define MAX_NODES 10
11
#define NODE_ATTRIBUTES 3
12 8

  
13 9
namespace amiro {
14 10

  
15
struct node {
16
  uint8_t id;
17
  uint8_t flag;
18
  uint8_t left;
19
  uint8_t right;
20
  uint8_t visited;
21
  types::position pL; // Left
22
  types::position pR; // Right
23
  types::position pB; // Back
24
};
11
  struct node {
12
    uint8_t id;
13
    uint8_t flag;
14
    uint8_t left;
15
    uint8_t right;
16
    uint8_t visited;
17
    types::position pL; // Left
18
    types::position pR; // Right
19
    types::position pB; // Back
20
  };
21

  
22
  struct map_state {
23
    // 0 - left, 1- right
24
    uint8_t strategy;
25
    // Node ID of last detected fixpoint
26
    uint8_t current;
27
    // Next node ID
28
    uint8_t next;
29
    //Traveled Distance between current and next in %
30
    uint8_t dist;
31
    // True if the current loaded map is valid
32
    bool valid;
33
  };
25 34

  
26 35
class AmiroMap {
27 36
public:
28
  uint8_t get_next()  { return next; }
29

  
30
  void set_next( uint8_t next) { this->next = next; }
31

  
32
  uint8_t get_current()  { return current; }
33

  
34
  void set_current( uint8_t current) { this->current = current; }
35

  
36
  node* get_nodeList()  { return nodeList; }
37

  
38
  uint8_t get_nodeCount()  { return nodeCount; }
39

  
40
  void set_nodeCount( uint8_t nodeCount) { this->nodeCount = nodeCount; }
41

  
42
  bool get_valid()  { return valid; }
43

  
44
  void set_valid( bool valid) { this->valid = valid; }
37
  map_state * get_state() { return &state; }
45 38

  
46 39
  AmiroMap(Global *global) : global{global} {}
47 40

  
......
50 43
   * @param config map configuration array
51 44
   *
52 45
   */
53
 uint8_t initialize(uint8_t (&config)[MAX_NODES][NODE_ATTRIBUTES]);
46
  uint8_t initialize();
54 47

  
55 48

  
56 49
  /**
57
     Update the internal map state according to the detected fixpoint
58
     This function should be called for a generic update, each can cycle and in
59
     case a fixpoint on one side is detected.
60
     Internal state maschine will go into an error state in case both sensors are black.
61

  
62
     @param left fixpoint on left side detected
63
     @param right fixpoint on right side detected
64
     @param strategy current line follow strategy
65

  
66

  
50
   * Update the internal map state according to the detected fixpoint
51
   * This function should be called for a generic update, each can cycle and in
52
   * case a fixpoint on one side is detected.
53
   * Internal state maschine will go into an error state in case both sensors are black.
54
   *
55
   *  @param left fixpoint on left side detected
56
   *  @param right fixpoint on right side detected
57
   *  @param strategy current line follow strategy
58
   *
59
   *
67 60
   */
68
  void update(bool left, bool right, LineFollowStrategy strategy);
61
  void update(LineFollowStrategy strategy);
69 62

  
70 63
private:
71 64
  Global *global;
72
  bool valid = false;
65
  LineFollowStrategy lfStrategy = LineFollowStrategy::EDGE_RIGHT;
66
  // Keeps track of the internal map state
67
  map_state state;
68
  // Holds the amount of detected node of the map
73 69
  uint8_t nodeCount = 0;
70
  // Stores all nodes and the corresponding coordinates of last detected fixpoints
74 71
  node nodeList[MAX_NODES];
75
  uint8_t current = 0;
76
  uint8_t next = 0;
72
  // If driving over fixpoint prevent continuous updating
73
  // True if left sensor is driving over black
74
  bool leftDetected = false;
75
  // True if right sensor is driving over black
76
  bool rightDetected = false;
77

  
78
  /**
79
   * Recursively search through all nodes in the node list and
80
   * mark all reached nodes as visited.
81
   *
82
   * @param id node from where to start visiting the other nodes
83
   *
84
   */
77 85
  void visitNode(uint8_t id);
78 86
  };
79 87
}; // namespace amiro

Also available in: Unified diff