Revision 6c9146d5 scout_avr/src/main.cpp
| scout_avr/src/main.cpp | ||
|---|---|---|
| 3 | 3 |
#include "ros.h" |
| 4 | 4 |
#include "bom.h" |
| 5 | 5 |
#include "range.h" |
| 6 |
#include <std_msgs/Int16.h> |
|
| 6 |
#include "stepper.h" |
|
| 7 |
#include <std_msgs/Int16.h> // TODO remove |
|
| 8 |
#include <bom/bom.h> |
|
| 9 |
#include <sonar/sonar_distance.h> |
|
| 10 |
#include <sonar/sonar_toggle.h> |
|
| 11 |
#include <sonar/sonar_set_scan.h> |
|
| 7 | 12 |
#include <util/delay.h> |
| 8 | 13 |
|
| 9 |
ros::Publisher *pbom_pub; |
|
| 14 |
/* Period of main loop in ms */ |
|
| 15 |
#define MAINLOOP_PERIOD 50 |
|
| 16 |
|
|
| 17 |
char range_enabled = 0; |
|
| 10 | 18 |
|
| 11 | 19 |
void debug(const char *str) |
| 12 | 20 |
{
|
| ... | ... | |
| 16 | 24 |
{
|
| 17 | 25 |
} |
| 18 | 26 |
|
| 27 |
/* dammit, Priya, this capitalization just looks ridiculous */ |
|
| 28 |
void range_toggle_cb(const sonar::sonar_toggleRequest& req, |
|
| 29 |
sonar::sonar_toggleResponse& resp) |
|
| 30 |
{
|
|
| 31 |
range_enabled = req.set_on; |
|
| 32 |
resp.ack = true; |
|
| 33 |
} |
|
| 34 |
|
|
| 35 |
void range_set_scan_cb(const sonar::sonar_set_scanRequest& req, |
|
| 36 |
sonar::sonar_set_scanResponse& resp) |
|
| 37 |
{
|
|
| 38 |
step_sweep_bounds(req.stop_l, req.stop_r); |
|
| 39 |
resp.ack = true; |
|
| 40 |
} |
|
| 41 |
|
|
| 19 | 42 |
int main() |
| 20 | 43 |
{
|
| 44 |
unsigned long now, next; |
|
| 45 |
unsigned int ranges[2]; |
|
| 21 | 46 |
char i, id; |
| 47 |
|
|
| 22 | 48 |
ros::NodeHandle nh; |
| 23 |
bom::bom bom_msg; |
|
| 24 |
ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
|
|
| 25 |
ros::Publisher bom_pub("bom", &bom_msg);
|
|
| 49 |
nh.initNode(); |
|
| 26 | 50 |
|
| 27 |
pbom_pub = &bom_pub; |
|
| 51 |
/* To be removed later; just an example of a subscirber */ |
|
| 52 |
ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
|
|
| 53 |
nh.subscribe(test_in); |
|
| 28 | 54 |
|
| 29 |
nh.initNode();
|
|
| 55 |
/* Range */
|
|
| 30 | 56 |
range_init(); |
| 57 |
sonar::sonar_distance range_msg; |
|
| 58 |
ros::Publisher range_distance("sonar_distance", &range_msg);
|
|
| 59 |
ros::ServiceServer |
|
| 60 |
<sonar::sonar_toggleRequest, sonar::sonar_toggleResponse> |
|
| 61 |
range_toggle("sonar_toggle", range_toggle_cb);
|
|
| 62 |
ros::ServiceServer |
|
| 63 |
<sonar::sonar_set_scanRequest, sonar::sonar_set_scanResponse> |
|
| 64 |
range_set_scan("sonar_set_scan", range_set_scan_cb);
|
|
| 65 |
|
|
| 66 |
/* BOM */ |
|
| 31 | 67 |
bom_init(); |
| 32 |
nh.subscribe(test_in); |
|
| 68 |
bom::bom bom_msg; |
|
| 69 |
ros::Publisher bom_pub("bom", &bom_msg);
|
|
| 33 | 70 |
nh.advertise(bom_pub); |
| 34 | 71 |
|
| 35 | 72 |
id = 0; |
| 73 |
next = 0; |
|
| 36 | 74 |
while (1) |
| 37 | 75 |
{
|
| 76 |
nh.spinOnce(); |
|
| 77 |
|
|
| 78 |
/* Skip loop if the loop period hasn't passed yet */ |
|
| 79 |
/* TODO if we need more exact timing, we can enter a tight loop when now |
|
| 80 |
* gets close to next, and avoid the uncertainty of nh.spinOnce() */ |
|
| 81 |
now = nh.getHardware()->time(); |
|
| 82 |
if (now < next) {
|
|
| 83 |
continue; |
|
| 84 |
} |
|
| 85 |
next = now + MAINLOOP_PERIOD; |
|
| 86 |
|
|
| 87 |
/* Temporary, for BOM testing */ |
|
| 38 | 88 |
id++; |
| 39 | 89 |
if (id == 0x40) {
|
| 40 | 90 |
id = 0; |
| 41 | 91 |
} |
| 42 | 92 |
set_robot_id(id); |
| 43 |
|
|
| 44 | 93 |
bom_send(id & 1); |
| 94 |
|
|
| 95 |
/* BOM */ |
|
| 45 | 96 |
for (i = 0; i < 4; i++) {
|
| 46 | 97 |
int msg = bom_get(i); |
| 47 | 98 |
if (msg != BOM_NO_DATA) {
|
| ... | ... | |
| 52 | 103 |
} |
| 53 | 104 |
} |
| 54 | 105 |
|
| 55 |
nh.spinOnce(); |
|
| 56 |
_delay_ms(500); |
|
| 106 |
/* Stepper / range sensor */ |
|
| 107 |
// TODO sweep stepper |
|
| 108 |
range_measure(ranges); |
|
| 109 |
range_msg.header.stamp = nh.now(); |
|
| 110 |
range_msg.header.seq++; |
|
| 111 |
range_msg.pos = 0; // TODO fill this |
|
| 112 |
range_msg.distance0 = ranges[0]; |
|
| 113 |
range_msg.distance1 = ranges[1]; |
|
| 114 |
|
|
| 57 | 115 |
} |
| 58 | 116 |
|
| 59 | 117 |
return 0; |
| ... | ... | |
| 72 | 130 |
#include "Atmega128rfa1.h" |
| 73 | 131 |
#include "range.h" |
| 74 | 132 |
#include "bom.h" |
| 133 |
#include "stepper.h" |
|
| 75 | 134 |
|
| 76 | 135 |
Atmega128rfa1 avr; |
| 77 | 136 |
|
| ... | ... | |
| 89 | 148 |
avr.init(); |
| 90 | 149 |
range_init(); |
| 91 | 150 |
bom_init(); |
| 151 |
func step_sweep_cb = step_init(10); |
|
| 152 |
step_sweep_bounds(-26, 26); |
|
| 153 |
step_dir(1); |
|
| 154 |
step_sweep_speed(50); |
|
| 92 | 155 |
avr.puts("Hello!\r\n");
|
| 93 | 156 |
while (1) |
| 94 | 157 |
{
|
| 95 |
now = avr.time(); |
|
| 158 |
step_sweep_cb(); |
|
| 159 |
_delay_ms(10); |
|
| 160 |
/*now = avr.time(); |
|
| 96 | 161 |
if (now > next) {
|
| 97 |
next = now + 500; |
|
| 162 |
next = now + 50;*/ |
|
| 163 |
//step_sweep_cb(); |
|
| 98 | 164 |
/*ultoa(now, buf, 10); |
| 99 | 165 |
avr.puts(buf); |
| 100 | 166 |
avr.puts("\r\n");*/
|
| 101 |
id++; |
|
| 167 |
/*id++;
|
|
| 102 | 168 |
if (id == 0x40) {
|
| 103 | 169 |
id = 0; |
| 104 | 170 |
} |
| 105 |
set_robot_id(id); |
|
| 171 |
set_robot_id(id);*/
|
|
| 106 | 172 |
/*utoa(range_get(0), buf, 10); |
| 107 | 173 |
avr.puts("Range: ");
|
| 108 | 174 |
avr.puts(buf); |
| 109 | 175 |
avr.puts(", ");
|
| 110 | 176 |
utoa(range_get(1), buf, 10); |
| 111 | 177 |
avr.puts(buf);*/ |
| 112 |
for (i = 0; i < 4; i++) {
|
|
| 178 |
/*for (i = 0; i < 4; i++) {
|
|
| 113 | 179 |
bom_send(i); |
| 114 | 180 |
int msg = bom_get(i); |
| 115 | 181 |
if (msg != BOM_NO_DATA) {
|
| ... | ... | |
| 124 | 190 |
avr.puts(buf); |
| 125 | 191 |
avr.puts(")\r\n");
|
| 126 | 192 |
} |
| 127 |
} |
|
| 128 |
} |
|
| 193 |
}*/
|
|
| 194 |
//}
|
|
| 129 | 195 |
} |
| 130 | 196 |
return 0; |
| 131 | 197 |
} |
Also available in: Unified diff