Revision 066b08bb scout_avr/src/main.cpp

View differences:

scout_avr/src/main.cpp
4 4
#include "bom.h"
5 5
#include "range.h"
6 6
#include "stepper.h"
7
#include "orb.h"
7 8
#include <std_msgs/Int16.h> // TODO remove
8 9
#include <bom/bom.h>
9 10
#include <sonar/sonar_distance.h>
10 11
#include <sonar/sonar_toggle.h>
11 12
#include <sonar/sonar_set_scan.h>
13
#include <headlights/set_headlights.h>
12 14
#include <util/delay.h>
13 15

  
14 16
/* Period of main loop in ms */
......
20 22
{
21 23
}
22 24

  
23
void callback(const std_msgs::Int16& msg)
25
void orb_callback(const headlights::set_headlights& msg)
24 26
{
27
  orb_set0(msg.left_red, msg.left_green, msg.left_blue);
28
  orb_set1(msg.right_red, msg.right_green, msg.right_blue);
25 29
}
26 30

  
27 31
/* dammit, Priya, this capitalization just looks ridiculous */
......
52 56
  ros::NodeHandle nh;
53 57
  nh.initNode();
54 58

  
55
  /* To be removed later; just an example of a subscirber */
56
  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
57
  nh.subscribe(test_in);
58

  
59 59
  /* Stepper */
60 60
  // TODO ROS messages to set bounds
61 61
  step_init();
......
83 83
  ros::Publisher bom_pub("bom", &bom_msg);
84 84
  nh.advertise(bom_pub);
85 85

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

  
86 92
  id = 0;
87 93
  next = 0;
88 94
  while (1)
......
104 110
      id = 0;
105 111
    }
106 112
    set_robot_id(id);
107
    bom_send(id & 1);
113
    bom_send(0);
108 114

  
109 115
    /* BOM */
110 116
    for (i = 0; i < 4; i++) {

Also available in: Unified diff