commit 6c9146d55fc6357be88eaa2185dd43e018001e75
Author: Tom Mullins <tmullins@andrew.cmu.edu>
Date:   Thu Nov 15 22:02:44 2012 -0500

    Began filling in ROSsy main function

diff --git a/scout_avr/src/main.cpp b/scout_avr/src/main.cpp
index ef6df38..d5d0c13 100644
--- a/scout_avr/src/main.cpp
+++ b/scout_avr/src/main.cpp
@@ -3,10 +3,18 @@
 #include "ros.h"
 #include "bom.h"
 #include "range.h"
-#include <std_msgs/Int16.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>
 
-ros::Publisher *pbom_pub;
+/* Period of main loop in ms */
+#define MAINLOOP_PERIOD 50
+
+char range_enabled = 0;
 
 void debug(const char *str)
 {
@@ -16,32 +24,75 @@ 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;
+  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;
-  bom::bom bom_msg;
-  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
-  ros::Publisher bom_pub("bom", &bom_msg);
+  nh.initNode();
 
-  pbom_pub = &bom_pub;
+  /* To be removed later; just an example of a subscirber */
+  ros::Subscriber<std_msgs::Int16> test_in("test_in", callback);
+  nh.subscribe(test_in);
 
-  nh.initNode();
+  /* Range */
   range_init();
+  sonar::sonar_distance range_msg;
+  ros::Publisher range_distance("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);
+
+  /* BOM */
   bom_init();
-  nh.subscribe(test_in);
+  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) {
@@ -52,8 +103,15 @@ int main()
       }
     }
 
-    nh.spinOnce();
-    _delay_ms(500);
+    /* Stepper / range sensor */
+    // TODO sweep stepper
+    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];
+
   }
 
   return 0;
@@ -72,6 +130,7 @@ extern "C"
 #include "Atmega128rfa1.h"
 #include "range.h"
 #include "bom.h"
+#include "stepper.h"
 
 Atmega128rfa1 avr;
 
@@ -89,27 +148,34 @@ int main()
   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);
   avr.puts("Hello!\r\n");
   while (1)
   {
-    now = avr.time();
+    step_sweep_cb();
+    _delay_ms(10);
+    /*now = avr.time();
     if (now > next) {
-      next = now + 500;
+      next = now + 50;*/
+      //step_sweep_cb();
       /*ultoa(now, buf, 10);
       avr.puts(buf);
       avr.puts("\r\n");*/
-      id++;
+      /*id++;
       if (id == 0x40) {
         id = 0;
       }
-      set_robot_id(id);
+      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++) {
+      /*for (i = 0; i < 4; i++) {
         bom_send(i);
         int msg = bom_get(i);
         if (msg != BOM_NO_DATA) {
@@ -124,8 +190,8 @@ int main()
           avr.puts(buf);
           avr.puts(")\r\n");
         }
-      }
-    }
+      }*/
+    //}
   }
   return 0;
 }
