Revision 066b08bb
Adding Charles's headlight code
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