scoutos / scout_avr / src / main.cpp @ 1b05b4cc
History | View | Annotate | Download (3.87 KB)
1 | 49090532 | Tom Mullins | #include "ros.h" |
---|---|---|---|
2 | 6e7f0a98 | Tom Mullins | #include "bom.h" |
3 | 47e26dee | Tom Mullins | #include "range.h" |
4 | 6c9146d5 | Tom Mullins | #include "stepper.h" |
5 | 066b08bb | Tom Mullins | #include "orb.h" |
6 | 8741d18c | Tom Mullins | #include "cliffSensor.h" |
7 | 0970d303 | Tom Mullins | #include <messages/bom.h> |
8 | #include <messages/sonar_distance.h> |
||
9 | #include <messages/sonar_toggle.h> |
||
10 | #include <messages/sonar_set_scan.h> |
||
11 | #include <messages/set_headlights.h> |
||
12 | #include <messages/cliff_status_changed.h> |
||
13 | 47e26dee | Tom Mullins | #include <util/delay.h> |
14 | 88fb3a79 | Tom Mullins | |
15 | 6c9146d5 | Tom Mullins | /* Period of main loop in ms */
|
16 | 2853d46b | Tom Mullins | #define MAINLOOP_PERIOD 100//50 |
17 | 6c9146d5 | Tom Mullins | |
18 | char range_enabled = 0; |
||
19 | 49090532 | Tom Mullins | |
20 | 0970d303 | Tom Mullins | void orb_callback(const messages::set_headlights& msg) |
21 | 49090532 | Tom Mullins | { |
22 | 066b08bb | Tom Mullins | orb_set0(msg.left_red, msg.left_green, msg.left_blue); |
23 | orb_set1(msg.right_red, msg.right_green, msg.right_blue); |
||
24 | 49090532 | Tom Mullins | } |
25 | |||
26 | 6c9146d5 | Tom Mullins | /* dammit, Priya, this capitalization just looks ridiculous */
|
27 | 0970d303 | Tom Mullins | void range_toggle_cb(const messages::sonar_toggleRequest& req, |
28 | messages::sonar_toggleResponse& resp) |
||
29 | 6c9146d5 | Tom Mullins | { |
30 | range_enabled = req.set_on; |
||
31 | 64aea12e | Tom Mullins | if (range_enabled)
|
32 | step_enable(); |
||
33 | else
|
||
34 | step_disable(); |
||
35 | 6c9146d5 | Tom Mullins | resp.ack = true;
|
36 | } |
||
37 | |||
38 | 0970d303 | Tom Mullins | void range_set_scan_cb(const messages::sonar_set_scanRequest& req, |
39 | messages::sonar_set_scanResponse& resp) |
||
40 | 6c9146d5 | Tom Mullins | { |
41 | step_sweep_bounds(req.stop_l, req.stop_r); |
||
42 | resp.ack = true;
|
||
43 | } |
||
44 | |||
45 | 49090532 | Tom Mullins | int main()
|
46 | 88fb3a79 | Tom Mullins | { |
47 | 6c9146d5 | Tom Mullins | unsigned long now, next; |
48 | unsigned int ranges[2]; |
||
49 | 4bdd00ba | Tom Mullins | char i, id, cliff_status;
|
50 | 6c9146d5 | Tom Mullins | |
51 | 88fb3a79 | Tom Mullins | ros::NodeHandle nh; |
52 | 6c9146d5 | Tom Mullins | nh.initNode(); |
53 | 49090532 | Tom Mullins | |
54 | 2853d46b | Tom Mullins | /* Stepper */
|
55 | // TODO ROS messages to set bounds
|
||
56 | step_init(); |
||
57 | step_dir(1);
|
||
58 | 53349043 | Tom Mullins | step_set_size(STEP_HALF); |
59 | 2853d46b | Tom Mullins | step_sweep_bounds(-26, 26); |
60 | |||
61 | 6c9146d5 | Tom Mullins | /* Range */
|
62 | 47e26dee | Tom Mullins | range_init(); |
63 | 0970d303 | Tom Mullins | messages::sonar_distance range_msg; |
64 | 86b48573 | Tom Mullins | ros::Publisher range_pub("sonar_distance", &range_msg);
|
65 | 6c9146d5 | Tom Mullins | ros::ServiceServer |
66 | 0970d303 | Tom Mullins | <messages::sonar_toggleRequest, messages::sonar_toggleResponse> |
67 | 6c9146d5 | Tom Mullins | range_toggle("sonar_toggle", range_toggle_cb);
|
68 | ros::ServiceServer |
||
69 | 0970d303 | Tom Mullins | <messages::sonar_set_scanRequest, messages::sonar_set_scanResponse> |
70 | 6c9146d5 | Tom Mullins | range_set_scan("sonar_set_scan", range_set_scan_cb);
|
71 | 86b48573 | Tom Mullins | nh.advertise(range_pub); |
72 | 958699af | Tom Mullins | nh.advertiseService(range_toggle); |
73 | nh.advertiseService(range_set_scan); |
||
74 | 6c9146d5 | Tom Mullins | |
75 | /* BOM */
|
||
76 | 47e26dee | Tom Mullins | bom_init(); |
77 | 0970d303 | Tom Mullins | messages::bom bom_msg; |
78 | 6c9146d5 | Tom Mullins | ros::Publisher bom_pub("bom", &bom_msg);
|
79 | 6e7f0a98 | Tom Mullins | nh.advertise(bom_pub); |
80 | 88fb3a79 | Tom Mullins | |
81 | 066b08bb | Tom Mullins | /* Headlights (aka Orbs) */
|
82 | 53349043 | Tom Mullins | //orb_init(); TODO debug orbs
|
83 | 4bdd00ba | Tom Mullins | ros::Subscriber<messages::set_headlights> orb_sub("set_headlights",
|
84 | 066b08bb | Tom Mullins | orb_callback); |
85 | 4bdd00ba | Tom Mullins | nh.subscribe(orb_sub); |
86 | 066b08bb | Tom Mullins | |
87 | 8741d18c | Tom Mullins | /* Cliff sensors */
|
88 | cliffSensor_init(); |
||
89 | 0970d303 | Tom Mullins | messages::cliff_status_changed cliff_msg; |
90 | 8741d18c | Tom Mullins | ros::Publisher cliff_pub("cliff", &cliff_msg);
|
91 | nh.advertise(cliff_pub); |
||
92 | |||
93 | 6e7f0a98 | Tom Mullins | id = 0;
|
94 | 6c9146d5 | Tom Mullins | next = 0;
|
95 | 88fb3a79 | Tom Mullins | while (1) |
96 | { |
||
97 | 6c9146d5 | Tom Mullins | nh.spinOnce(); |
98 | |||
99 | /* Skip loop if the loop period hasn't passed yet */
|
||
100 | /* TODO if we need more exact timing, we can enter a tight loop when now
|
||
101 | * gets close to next, and avoid the uncertainty of nh.spinOnce() */
|
||
102 | now = nh.getHardware()->time(); |
||
103 | if (now < next) {
|
||
104 | continue;
|
||
105 | } |
||
106 | next = now + MAINLOOP_PERIOD; |
||
107 | |||
108 | /* Temporary, for BOM testing */
|
||
109 | 6e7f0a98 | Tom Mullins | id++; |
110 | if (id == 0x40) { |
||
111 | id = 0;
|
||
112 | } |
||
113 | set_robot_id(id); |
||
114 | 066b08bb | Tom Mullins | bom_send(0);
|
115 | 6c9146d5 | Tom Mullins | |
116 | /* BOM */
|
||
117 | 6e7f0a98 | Tom Mullins | for (i = 0; i < 4; i++) { |
118 | int msg = bom_get(i);
|
||
119 | if (msg != BOM_NO_DATA) {
|
||
120 | bom_msg.sender = bom_msg_get_robot_id(msg); |
||
121 | bom_msg.send_dir = bom_msg_get_dir(msg); |
||
122 | bom_msg.recv_dir = i; |
||
123 | bom_pub.publish(&bom_msg); |
||
124 | } |
||
125 | } |
||
126 | 47e26dee | Tom Mullins | |
127 | 6c9146d5 | Tom Mullins | /* Stepper / range sensor */
|
128 | 2853d46b | Tom Mullins | if (range_enabled) {
|
129 | step_sweep(); |
||
130 | range_measure(ranges); |
||
131 | 8741d18c | Tom Mullins | range_msg.stamp = nh.now(); |
132 | daf6a575 | Tom Mullins | range_msg.pos = step_get_pos(); |
133 | 2853d46b | Tom Mullins | range_msg.distance0 = ranges[0];
|
134 | range_msg.distance1 = ranges[1];
|
||
135 | c06735bb | Hui Jun Tay | range_msg.stamp = nh.now(); |
136 | 2853d46b | Tom Mullins | range_pub.publish(&range_msg); |
137 | } |
||
138 | 6c9146d5 | Tom Mullins | |
139 | 4bdd00ba | Tom Mullins | cliff_status = 0;
|
140 | if (read_cliffSensor_front())
|
||
141 | cliff_status |= messages::cliff_status_changed::CLIFF_FRONT; |
||
142 | if (read_cliffSensor_left())
|
||
143 | cliff_status |= messages::cliff_status_changed::CLIFF_LEFT; |
||
144 | if (read_cliffSensor_right())
|
||
145 | cliff_status |= messages::cliff_status_changed::CLIFF_RIGHT; |
||
146 | if (cliff_status != cliff_msg.cliff_status) {
|
||
147 | cliff_msg.cliff_status = cliff_status; |
||
148 | 0970d303 | Tom Mullins | cliff_pub.publish(&cliff_msg); |
149 | } |
||
150 | 8741d18c | Tom Mullins | |
151 | 88fb3a79 | Tom Mullins | } |
152 | |||
153 | return 0; |
||
154 | 49090532 | Tom Mullins | } |