Colony » Colony Scout

#if 0 ///////////////////////////////////////////////

#include "ros.h"

#include <std_msgs/Int16.h>

ros::Publisher *ptest_out;

void callback(const std_msgs::Int16& msg)

{

  ptest_out->publish(&msg);

}

int main()

{

  ros::NodeHandle nh;

  std_msgs::Int16 msg;

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

  ros::Publisher test_out("test_out", &msg);

  ptest_out = &test_out;

  msg.data = 0;

  nh.initNode();

  nh.subscribe(test_in);

  nh.advertise(test_out);

  while (1)

  {

    nh.spinOnce();

  }

  return 0;

}

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

extern "C"

{

#include <stdlib.h>

#include <string.h>

}

#include "Atmega128rfa1.h"

#include "range.h"

int main()

{

  char buf[20];

  unsigned long now, next = 0;

  Atmega128rfa1 avr;

  avr.init();

  range_init();

  while (1)

  {

    now = avr.time();

    if (now > next) {

      next = now + 100;

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

      avr.write((uint8_t*) buf, strlen(buf));

      avr.write((uint8_t*) ", ", 2);

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

      avr.write((uint8_t*) buf, strlen(buf));

      /*int data = avr.read();

      if (data >= 0)

      {

        uint8_t data8 = data;

        avr.write((uint8_t*) "Received ", 9);

        avr.write(&data8, 1);

      }

      else

        avr.write((uint8_t*) ".", 1);*/

      avr.write((uint8_t*) "\r\n", 2);

    }

  }

  return 0;

}

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