Colony » Colony Scout

#if 1 ///////////////////////////////////////////////

#include "ros.h"

#include "bom.h"

#include <std_msgs/Int16.h>

ros::Publisher *pbom_pub;

void debug(const char *str)

{

}

void callback(const std_msgs::Int16& msg)

{

}

int main()

{

  char i, id;

  ros::NodeHandle nh;

  bom::bom bom_msg;

  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);

  ros::Publisher bom_pub("bom", &bom_msg);

  pbom_pub = &bom_pub;

  nh.initNode();

  nh.subscribe(test_in);

  nh.advertise(bom_pub);

  id = 0;

  while (1)

  {

    id++;

    if (id == 0x40) {

      id = 0;

    }

    set_robot_id(id);

    for (i = 0; i < 4; i++) {

      bom_send(i);

      int msg = bom_get(i);

      if (msg != BOM_NO_DATA) {

        bom_msg.sender = bom_msg_get_robot_id(msg);

        bom_msg.send_dir = bom_msg_get_dir(msg);

        bom_msg.recv_dir = i;

        bom_pub.publish(&bom_msg);

      }

    }

    nh.spinOnce();

  }

  return 0;

}

#else ///////////////////////////////////////////////

extern "C"

{

#include <stdlib.h>

#include <string.h>

#include <avr/io.h>

#include <util/delay.h>

}

#include "Atmega128rfa1.h"

#include "range.h"

#include "bom.h"

Atmega128rfa1 avr;

void debug(const char *str)

{

  avr.puts(str);

}

int main()

{

  char buf[20];

  int i;

  char id = 0;

  unsigned long now, next = 0;

  avr.init();

  range_init();

  bom_init();

  avr.puts("Hello!\r\n");

  while (1)

  {

    now = avr.time();

    if (now > next) {

      next = now + 500;

      /*ultoa(now, buf, 10);

      avr.puts(buf);

      avr.puts("\r\n");*/

      id++;

      if (id == 0x40) {

        id = 0;

      }

      set_robot_id(id);

      /*utoa(range_get(0), buf, 10);

      avr.puts("Range: ");

      avr.puts(buf);

      avr.puts(", ");

      utoa(range_get(1), buf, 10);

      avr.puts(buf);*/

      for (i = 0; i < 4; i++) {

        bom_send(i);

        int msg = bom_get(i);

        if (msg != BOM_NO_DATA) {

          avr.puts("BOM ");

          itoa(i, buf, 10);

          avr.puts(buf);

          avr.puts(": ");

          itoa(bom_msg_get_robot_id(msg), buf, 10);

          avr.puts(buf);

          avr.puts(" (");

          itoa(bom_msg_get_dir(msg), buf, 10);

          avr.puts(buf);

          avr.puts(")\r\n");

        }

      }

    }

  }

  return 0;

}

#endif //////////////////////////////////////////////