Revision 8741d18c
ID | 8741d18c83107749af09241be670bde145f42876 |
Added cliffsensor/ROS code to main file (untested)
scout_avr/Makefile | ||
---|---|---|
16 | 16 |
HDR=src/*.h |
17 | 17 |
FLAGS=-Isrc/ros_lib -Isrc -mmcu=$(MCU) -DF_CPU=$(F_CPU)UL -funsigned-char -Os -fpack-struct -Wall |
18 | 18 |
|
19 |
ROS_MSG_DEPS=rosserial_msgs std_msgs bom sonar headlights |
|
19 |
ROS_MSG_DEPS=rosserial_msgs std_msgs bom sonar headlights cliffsensor
|
|
20 | 20 |
|
21 | 21 |
default: scout_avr.hex |
22 | 22 |
|
scout_avr/src/main.cpp | ||
---|---|---|
5 | 5 |
#include "range.h" |
6 | 6 |
#include "stepper.h" |
7 | 7 |
#include "orb.h" |
8 |
#include "cliffSensor.h" |
|
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> |
12 | 13 |
#include <headlights/set_headlights.h> |
14 |
#include <cliffsensor/cliff_status_changed.h> |
|
13 | 15 |
#include <util/delay.h> |
14 | 16 |
|
15 | 17 |
/* Period of main loop in ms */ |
... | ... | |
88 | 90 |
orb_callback); |
89 | 91 |
nh.subscribe(orb_sub); |
90 | 92 |
|
93 |
/* Cliff sensors */ |
|
94 |
cliffSensor_init(); |
|
95 |
cliffsensor::cliff_status_changed cliff_msg; |
|
96 |
ros::Publisher cliff_pub("cliff", &cliff_msg); |
|
97 |
nh.advertise(cliff_pub); |
|
98 |
|
|
91 | 99 |
id = 0; |
92 | 100 |
next = 0; |
93 | 101 |
while (1) |
... | ... | |
126 | 134 |
if (range_enabled) { |
127 | 135 |
step_sweep(); |
128 | 136 |
range_measure(ranges); |
129 |
range_msg.header.stamp = nh.now(); |
|
130 |
range_msg.header.seq++; |
|
137 |
range_msg.stamp = nh.now(); |
|
131 | 138 |
range_msg.pos = step_get_pos(); |
132 | 139 |
range_msg.distance0 = ranges[0]; |
133 | 140 |
range_msg.distance1 = ranges[1]; |
... | ... | |
135 | 142 |
range_pub.publish(&range_msg); |
136 | 143 |
} |
137 | 144 |
|
145 |
/* TODO remove raw values and have single bitmask */ |
|
146 |
cliff_msg.front_raw = read_cliffSensor_front(); |
|
147 |
cliff_msg.left_raw = read_cliffSensor_left(); |
|
148 |
cliff_msg.right_raw = read_cliffSensor_right(); |
|
149 |
cliff_msg.cliff_status = cliff_msg.front_raw | cliff_msg.left_raw |
|
150 |
| cliff_msg.right_raw; |
|
151 |
|
|
138 | 152 |
} |
139 | 153 |
|
140 | 154 |
return 0; |
Also available in: Unified diff