Colony » Colony Scout » Colony Scout OS

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

#include "ros.h"

#include "bom.h"

#include "range.h"

#include "stepper.h"

#include <std_msgs/Int16.h> // TODO remove

#include <bom/bom.h>

#include <sonar/sonar_distance.h>

#include <sonar/sonar_toggle.h>

#include <sonar/sonar_set_scan.h>

#include <util/delay.h>

/* Period of main loop in ms */

#define MAINLOOP_PERIOD 100//50

char range_enabled = 0;

void debug(const char *str)

{

}

void callback(const std_msgs::Int16& msg)

{

}

/* dammit, Priya, this capitalization just looks ridiculous */

void range_toggle_cb(const sonar::sonar_toggleRequest& req,

    sonar::sonar_toggleResponse& resp)

{

  range_enabled = req.set_on;

  if (range_enabled)

    step_enable();

  else

    step_disable();

  resp.ack = true;

}

void range_set_scan_cb(const sonar::sonar_set_scanRequest& req,

    sonar::sonar_set_scanResponse& resp)

{

  step_sweep_bounds(req.stop_l, req.stop_r);

  resp.ack = true;

}

int main()

{

  unsigned long now, next;

  unsigned int ranges[2];

  char i, id;

  ros::NodeHandle nh;

  nh.initNode();

  /* To be removed later; just an example of a subscirber */

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

  nh.subscribe(test_in);

  /* Stepper */

  // TODO ROS messages to set bounds

  step_init();

  step_dir(1);

  step_set_size(STEP_WHOLE);

  step_sweep_bounds(-26, 26);

  /* Range */

  range_init();

  sonar::sonar_distance range_msg;

  ros::Publisher range_pub("sonar_distance", &range_msg);

  ros::ServiceServer

    <sonar::sonar_toggleRequest, sonar::sonar_toggleResponse>

    range_toggle("sonar_toggle", range_toggle_cb);

  ros::ServiceServer

    <sonar::sonar_set_scanRequest, sonar::sonar_set_scanResponse>

    range_set_scan("sonar_set_scan", range_set_scan_cb);

  nh.advertise(range_pub);

  nh.advertiseService(range_toggle);

  nh.advertiseService(range_set_scan);

  /* BOM */

  bom_init();

  bom::bom bom_msg;

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

  nh.advertise(bom_pub);

  id = 0;

  next = 0;

  while (1)

  {

    nh.spinOnce();

    /* Skip loop if the loop period hasn't passed yet */

    /* TODO if we need more exact timing, we can enter a tight loop when now

     * gets close to next, and avoid the uncertainty of nh.spinOnce() */

    now = nh.getHardware()->time();

    if (now < next) {

      continue;

    }

    next = now + MAINLOOP_PERIOD;

    /* Temporary, for BOM testing */

    id++;

    if (id == 0x40) {

      id = 0;

    }

    set_robot_id(id);

    bom_send(id & 1);

    /* BOM */

    for (i = 0; i < 4; 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);

      }

    }

    /* Stepper / range sensor */

    if (range_enabled) {

      step_sweep();

      range_measure(ranges);

      range_msg.header.stamp = nh.now();

      range_msg.header.seq++;

      range_msg.pos = 0; // TODO fill this

      range_msg.distance0 = ranges[0];

      range_msg.distance1 = ranges[1];

      range_pub.publish(&range_msg);

    }

  }

  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"

#include "stepper.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;

  unsigned int ranges[2];*/

  avr.init();

  range_init();

  bom_init();

  func step_sweep_cb = step_init(10);

  step_sweep_bounds(-26, 26);

  step_dir(1);

  step_sweep_speed(50);

  PORTF |= _BV(PF4);

  _delay_ms(500);

  PORTF = (PORTF & ~_BV(PF4)) | _BV(PF5);

  _delay_ms(500);

  PORTF &= ~_BV(PF5);

  while (1)

  {

    step_sweep_cb();

    _delay_ms(10);

    /*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);*/

      /*range_measure(ranges);

      utoa(ranges[0], buf, 10);

      avr.puts("Range: ");

      avr.puts(buf);

      avr.puts(", ");

      utoa(ranges[1], buf, 10);

      avr.puts(buf);

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

      /*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 //////////////////////////////////////////////