Project

General

Profile

Revision 6e7f0a98

ID6e7f0a982e8b5aa27826dc589fbabf19bef59990

Added by Thomas Mullins over 11 years ago

Changed scout_avr's main.cpp to use rosserial

View differences:

scout_avr/src/bom.h
5 5
#include <stdint.h>
6 6
}
7 7

  
8
//#include <bom/bom.h>
8
#include <bom/bom.h>
9 9

  
10 10
// constants for direction
11
#define BOM_FRONT 0 //(bom::bom::FRONT)
12
#define BOM_BACK  1 //(bom::bom::BACK)
13
#define BOM_LEFT  2 //(bom::bom::LEFT)
14
#define BOM_RIGHT 3 //(bom::bom::RIGHT)
11
#define BOM_FRONT (bom::bom::FRONT)
12
#define BOM_BACK  (bom::bom::BACK)
13
#define BOM_LEFT  (bom::bom::LEFT)
14
#define BOM_RIGHT (bom::bom::RIGHT)
15 15

  
16 16
// timing, in us, for read of valid bits
17 17
#define MIN_LOW_PW 600
scout_avr/src/main.cpp
1
#if 0 ///////////////////////////////////////////////
1
#if 1 ///////////////////////////////////////////////
2 2

  
3 3
#include "ros.h"
4
#include "bom.h"
5

  
4 6
#include <std_msgs/Int16.h>
5 7

  
6
ros::Publisher *ptest_out;
8
ros::Publisher *pbom_pub;
7 9

  
8 10
void debug(const char *str)
9 11
{
......
11 13

  
12 14
void callback(const std_msgs::Int16& msg)
13 15
{
14
  ptest_out->publish(&msg);
15 16
}
16 17

  
17 18
int main()
18 19
{
20
  char i, id;
19 21
  ros::NodeHandle nh;
20
  std_msgs::Int16 msg;
22
  bom::bom bom_msg;
21 23
  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
22
  ros::Publisher test_out("test_out", &msg);
24
  ros::Publisher bom_pub("bom", &bom_msg);
23 25

  
24
  ptest_out = &test_out;
26
  pbom_pub = &bom_pub;
25 27

  
26
  msg.data = 0;
27 28
  nh.initNode();
28 29
  nh.subscribe(test_in);
29
  nh.advertise(test_out);
30
  nh.advertise(bom_pub);
30 31

  
32
  id = 0;
31 33
  while (1)
32 34
  {
35
    id++;
36
    if (id == 0x40) {
37
      id = 0;
38
    }
39
    set_robot_id(id);
40
    for (i = 0; i < 4; i++) {
41
      bom_send(i);
42
      int msg = bom_get(i);
43
      if (msg != BOM_NO_DATA) {
44
        bom_msg.sender = bom_msg_get_robot_id(msg);
45
        bom_msg.send_dir = bom_msg_get_dir(msg);
46
        bom_msg.recv_dir = i;
47
        bom_pub.publish(&bom_msg);
48
      }
49
    }
33 50
    nh.spinOnce();
34 51
  }
35 52

  
......
71 88
  {
72 89
    now = avr.time();
73 90
    if (now > next) {
74
      next = now + 100;
91
      next = now + 500;
75 92
      /*ultoa(now, buf, 10);
76 93
      avr.puts(buf);
77 94
      avr.puts("\r\n");*/

Also available in: Unified diff