Revision 4bdd00ba scout_avr/src/main.cpp

View differences:

scout_avr/src/main.cpp
52 52
{
53 53
  unsigned long now, next;
54 54
  unsigned int ranges[2];
55
  char i, id;
56
  int cliff_front, cliff_left, cliff_right;
55
  char i, id, cliff_status;
57 56

  
58 57
  ros::NodeHandle nh;
59 58
  nh.initNode();
......
86 85
  nh.advertise(bom_pub);
87 86

  
88 87
  /* Headlights (aka Orbs) */
89
  orb_init();
90
  /*ros::Subscriber<messages::set_headlights> orb_sub("set_headlights",
88
  //orb_init();
89
  ros::Subscriber<messages::set_headlights> orb_sub("set_headlights",
91 90
      orb_callback);
92
  nh.subscribe(orb_sub);*/
91
  nh.subscribe(orb_sub);
93 92

  
94 93
  /* Cliff sensors */
95 94
  cliffSensor_init();
......
143 142
      range_pub.publish(&range_msg);
144 143
    }
145 144

  
146
    /* TODO remove raw values and have single bitmask */
147
    cliff_front = read_cliffSensor_front();
148
    cliff_left = read_cliffSensor_left();
149
    cliff_right = read_cliffSensor_right();
150
    if (cliff_front != cliff_msg.front_raw || cliff_left != cliff_msg.left_raw
151
        || cliff_right != cliff_msg.right_raw) {
152
      cliff_msg.front_raw = cliff_front;
153
      cliff_msg.left_raw = cliff_left;
154
      cliff_msg.right_raw = cliff_right;
155
      cliff_msg.cliff_status = cliff_front || cliff_left || cliff_right;
145
    cliff_status = 0;
146
    if (read_cliffSensor_front())
147
      cliff_status |= messages::cliff_status_changed::CLIFF_FRONT;
148
    if (read_cliffSensor_left())
149
      cliff_status |= messages::cliff_status_changed::CLIFF_LEFT;
150
    if (read_cliffSensor_right())
151
      cliff_status |= messages::cliff_status_changed::CLIFF_RIGHT;
152
    if (cliff_status != cliff_msg.cliff_status) {
153
      cliff_msg.cliff_status = cliff_status;
156 154
      cliff_pub.publish(&cliff_msg);
157 155
    }
158 156

  

Also available in: Unified diff