commit 86b48573fdb86a6acb04eb3a1e9de80b22f0791e
Author: Tom Mullins <tmullins@andrew.cmu.edu>
Date:   Mon Nov 19 15:12:20 2012 -0500

    Range sensors work now! :D

diff --git a/scout_avr/src/main.cpp b/scout_avr/src/main.cpp
index d5d0c13..89c1fe8 100644
--- a/scout_avr/src/main.cpp
+++ b/scout_avr/src/main.cpp
@@ -12,7 +12,7 @@
 #include <util/delay.h>
 
 /* Period of main loop in ms */
-#define MAINLOOP_PERIOD 50
+#define MAINLOOP_PERIOD 1000//50
 
 char range_enabled = 0;
 
@@ -55,13 +55,14 @@ int main()
   /* Range */
   range_init();
   sonar::sonar_distance range_msg;
-  ros::Publisher range_distance("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);
 
   /* BOM */
   bom_init();
@@ -111,6 +112,7 @@ int main()
     range_msg.pos = 0; // TODO fill this
     range_msg.distance0 = ranges[0];
     range_msg.distance1 = ranges[1];
+    range_pub.publish(&range_msg);
 
   }
 
@@ -142,24 +144,25 @@ void debug(const char *str)
 int main()
 {
   char buf[20];
-  int i;
-  char id = 0;
+  //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);
+  /*func step_sweep_cb = step_init(10);
   step_sweep_bounds(-26, 26);
   step_dir(1);
-  step_sweep_speed(50);
+  step_sweep_speed(50);*/
   avr.puts("Hello!\r\n");
   while (1)
   {
-    step_sweep_cb();
-    _delay_ms(10);
-    /*now = avr.time();
+    /*step_sweep_cb();
+    _delay_ms(10);*/
+    now = avr.time();
     if (now > next) {
-      next = now + 50;*/
+      next = now + 500;
       //step_sweep_cb();
       /*ultoa(now, buf, 10);
       avr.puts(buf);
@@ -169,12 +172,14 @@ int main()
         id = 0;
       }
       set_robot_id(id);*/
-      /*utoa(range_get(0), buf, 10);
+      range_measure(ranges);
+      utoa(ranges[0], buf, 10);
       avr.puts("Range: ");
       avr.puts(buf);
       avr.puts(", ");
-      utoa(range_get(1), buf, 10);
-      avr.puts(buf);*/
+      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);
@@ -191,7 +196,7 @@ int main()
           avr.puts(")\r\n");
         }
       }*/
-    //}
+    }
   }
   return 0;
 }
diff --git a/scout_avr/src/range.cpp b/scout_avr/src/range.cpp
index 4150ed9..c6e583a 100644
--- a/scout_avr/src/range.cpp
+++ b/scout_avr/src/range.cpp
@@ -14,13 +14,13 @@ extern "C"
  * 37.5ms * 8 MHz / 8 prescaler = 37500 max wait
  * 37.5ms * 16 MHz / 8 prescaler = problem
  * 37.5ms * 16 MHz / 64 prescaler = 9375 max wait
+ * 147us/in * 16 MHz / 64 prescaler = 1.44685039 ticks / mm
  */
 
 struct range_t {
   unsigned int start; // timer value on rising edge
   unsigned int value; // last measured range
-  char started;
-  char done;
+  char busy;
 } volatile range[2];
 
 static void on_edge(int which)
@@ -40,13 +40,13 @@ static void on_edge(int which)
   if (int_high)
   {
     range[which].start = time;
-    range[which].started = 1;
+    range[which].busy = 1;
   }
   else
   {
     // if timer overflowed since start, this arithmetic should still work out
     range[which].value = time - range[which].start;
-    range[which].done = 1;
+    range[which].busy = 0;
   }
 }
 
@@ -83,20 +83,17 @@ void range_measure(unsigned int *values)
   for (i = 0; i < 2; i++)
   {
     range[i].value = RANGE_ERR;
-    range[i].started = 0;
-    range[i].done = 0;
+    range[i].busy = 0;
   }
 
   // TODO ensure that one interrupt won't be delayed because of the other
   PORT_SONAR_TX |= _BV(P_SONAR_TX);
-  _delay_ms(1);
+  _delay_ms(40);
   PORT_SONAR_TX &= ~ _BV(P_SONAR_TX);
 
   for (i = 0; i < 2; i++)
   {
-    if (range[i].started) {
-      while (!range[i].done) {}
-      values[i] = range[i].value;
-    }
+    while (range[i].busy) {}
+    values[i] = range[i].value;
   }
 }
