changeset 187:b716d7cb9c78 main

Pull from upstream.
author Bob Cook <bob@bobcookdev.com>
date Sat, 08 Jun 2013 11:50:52 -0700
parents 4ef780aebd7e (current diff) a84d43219fbc (diff)
children 6f0418bfd0a4
files
diffstat 22 files changed, 2232 insertions(+), 13 deletions(-) [+]
line wrap: on
line diff
--- a/main/demos/avr/ls7366-demo/main.cpp	Sat Jun 08 11:50:20 2013 -0700
+++ b/main/demos/avr/ls7366-demo/main.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -120,7 +120,9 @@
     uint32_t cntr_last = 0;
 
     uint32_t last_diffs[ 10 ];
-    uint32_t index = 0;
+    uint8_t  index = 0;
+    
+    uint32_t total_ticks = 0;
 
     for ( ;; )
     {
@@ -129,23 +131,26 @@
         uint8_t  dir;
         uint32_t diff = ls7366_compute_diff( cntr_last, cntr, &dir );
 
+        total_ticks += diff;
+
         last_diffs[ index++ ] = diff;
         index %= 10;
 
         if ( index == 0 )
         {
-            uint32_t sum = 0;
+            uint32_t total_per_second = 0;
 
             for ( uint8_t i = 0; i < 10; ++i )
             {
-                sum += last_diffs[ i ];
+                total_per_second += last_diffs[ i ];
             }
 
-            uint32_t avg = ( sum + 51 ) / 100;
+            uint32_t rpm = total_per_second * 100 / 30900L;
 
-            printf_P( PSTR("\r average %ld (%ld per second) dir: %s        "),
-                      avg,
-                      sum,
+            printf_P( PSTR("\r total %ld rpm %ld (%ld per second) dir: %s        "),
+                      total_ticks,
+                      rpm,
+                      total_per_second,
                       ( dir == 0 ? "up" : "down" ) );
         }
 
--- a/main/packages/avr/sensors/ls7366/ls7366.cpp	Sat Jun 08 11:50:20 2013 -0700
+++ b/main/packages/avr/sensors/ls7366/ls7366.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -289,6 +289,75 @@
 
 #endif // #if defined( PRJ_LS7366_ENABLE_CHIP_1 )
 
+#if defined( PRJ_LS7366_ENABLE_CHIP_2 )
+
+void ls7366_reset_2()
+{
+    select_chip_2();
+    spi_write( LS7366_CMD_CLEAR_STR );
+    deselect_chip_2();
+    
+    select_chip_2();
+    spi_write( LS7366_CMD_CLEAR_MDR0 );
+    deselect_chip_2();
+
+    select_chip_2();
+    spi_write( LS7366_CMD_CLEAR_MDR1 );
+    deselect_chip_2();
+
+    select_chip_2();
+    spi_write( LS7366_CMD_CLEAR_CNTR );
+    deselect_chip_2();
+}
+
+#endif // #if defined( PRJ_LS7366_ENABLE_CHIP_2 )
+
+#if defined( PRJ_LS7366_ENABLE_CHIP_3 )
+
+void ls7366_reset_3()
+{
+    select_chip_3();
+    spi_write( LS7366_CMD_CLEAR_STR );
+    deselect_chip_3();
+    
+    select_chip_3();
+    spi_write( LS7366_CMD_CLEAR_MDR0 );
+    deselect_chip_3();
+
+    select_chip_3();
+    spi_write( LS7366_CMD_CLEAR_MDR1 );
+    deselect_chip_3();
+
+    select_chip_3();
+    spi_write( LS7366_CMD_CLEAR_CNTR );
+    deselect_chip_3();
+}
+
+#endif // #if defined( PRJ_LS7366_ENABLE_CHIP_3 )
+
+#if defined( PRJ_LS7366_ENABLE_CHIP_4 )
+
+void ls7366_reset_4()
+{
+    select_chip_4();
+    spi_write( LS7366_CMD_CLEAR_STR );
+    deselect_chip_4();
+    
+    select_chip_4();
+    spi_write( LS7366_CMD_CLEAR_MDR0 );
+    deselect_chip_4();
+
+    select_chip_4();
+    spi_write( LS7366_CMD_CLEAR_MDR1 );
+    deselect_chip_4();
+
+    select_chip_4();
+    spi_write( LS7366_CMD_CLEAR_CNTR );
+    deselect_chip_4();
+}
+
+#endif // #if defined( PRJ_LS7366_ENABLE_CHIP_4 )
+
 // ----------------------------------------------------------------------------------------
 
 #if defined( PRJ_LS7366_ENABLE_CHIP_1 )
--- a/main/packages/common/can/can_messages.h	Sat Jun 08 11:50:20 2013 -0700
+++ b/main/packages/common/can/can_messages.h	Sat Jun 08 11:50:52 2013 -0700
@@ -144,14 +144,27 @@
 
 // ----------------------------------------------------------------------------------------
 
-const uint16_t can_dataid_motor_speed = 0x0120;
+const uint16_t can_dataid_wheel_speed = 0x0120;
 
 typedef struct
 {
-    int8_t motor_front;
-    int8_t motor_rear;
+    int8_t rpm_front;   // positive values indicate forward, negative reverse
+    int8_t rpm_rear;
+
+} can_data_wheel_speed;
+
+// ----------------------------------------------------------------------------------------
+
+const uint16_t can_dataid_wheel_status = 0x0121;
 
-} can_data_motor_speed;
+typedef struct
+{
+    int8_t actual_rpm_front;
+    int8_t actual_rpm_rear;
+    int8_t actual_motor_front;
+    int8_t actual_motor_rear;
+
+} can_data_wheel_status;
 
 // ----------------------------------------------------------------------------------------
 
--- a/main/packages/common/util/crc16.cpp	Sat Jun 08 11:50:20 2013 -0700
+++ b/main/packages/common/util/crc16.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -39,10 +39,16 @@
 //
 // ----------------------------------------------------------------------------------------
 
+#include "project_defs.h"
+
 #include "crc16.h"
 
 // ----------------------------------------------------------------------------------------
 
+#if defined( PRJ_UTIL_CRC16_ENABLE )
+
+// ----------------------------------------------------------------------------------------
+
 #if defined( __AVR__ )
 
 #include <avr/pgmspace.h>
@@ -107,3 +113,7 @@
 
 // ----------------------------------------------------------------------------------------
 
+#endif // #if defined( PRJ_UTIL_CRC16_ENABLE )
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/packages/common/util/crc16.h	Sat Jun 08 11:50:20 2013 -0700
+++ b/main/packages/common/util/crc16.h	Sat Jun 08 11:50:52 2013 -0700
@@ -35,6 +35,15 @@
 #include <stdint.h>
 
 // ----------------------------------------------------------------------------------------
+//  definitions from "project_defs.h"
+//
+//  enable CRC-16 computations
+//
+//      + PRJ_UTIL_CRC16_ENABLE
+//
+// ----------------------------------------------------------------------------------------
+
+// ----------------------------------------------------------------------------------------
 //  crc16
 //
 //      An object that accumulates the CRC-16 value over multiple function calls.
--- a/main/packages/common/util/crc32.cpp	Sat Jun 08 11:50:20 2013 -0700
+++ b/main/packages/common/util/crc32.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -39,10 +39,16 @@
 //
 // ----------------------------------------------------------------------------------------
 
+#include "project_defs.h"
+
 #include "crc32.h"
 
 // ----------------------------------------------------------------------------------------
 
+#if defined( PRJ_UTIL_CRC32_ENABLE )
+
+// ----------------------------------------------------------------------------------------
+
 #if defined( __AVR__ )
 
 #include <avr/pgmspace.h>
@@ -98,3 +104,7 @@
 
 // ----------------------------------------------------------------------------------------
 
+#endif // #if defined( PRJ_UTIL_CRC32_ENABLE )
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/packages/common/util/crc32.h	Sat Jun 08 11:50:20 2013 -0700
+++ b/main/packages/common/util/crc32.h	Sat Jun 08 11:50:52 2013 -0700
@@ -35,6 +35,15 @@
 #include <stdint.h>
 
 // ----------------------------------------------------------------------------------------
+//  definitions from "project_defs.h"
+//
+//  enable CRC-32 computations
+//
+//      + PRJ_UTIL_CRC32_ENABLE
+//
+// ----------------------------------------------------------------------------------------
+
+// ----------------------------------------------------------------------------------------
 //  crc32
 //
 //      An object that accumulates the CRC-32 value over multiple functions.
--- a/main/packages/common/util/printutils.cpp	Sat Jun 08 11:50:20 2013 -0700
+++ b/main/packages/common/util/printutils.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -7,7 +7,7 @@
 //    
 //  This file provides printf-helper functions and macros.
 //
-//  Copyright (c) 2011 Bob Cook
+//  Copyright (c) 2011-2013 Bob Cook
 //  
 //  Permission is hereby granted, free of charge, to any person obtaining a copy
 //  of this software and associated documentation files (the "Software"), to deal
@@ -33,13 +33,35 @@
 
 #include "project_defs.h"
 
-#if defined( PRJ_PRINTHEX_ENABLE_OUTPUT_TO_STDOUT ) \
+#if defined( PRJ_PRINTDEC_ENABLE_OUTPUT_TO_STDOUT ) \
+    || defined( PRJ_PRINTHEX_ENABLE_OUTPUT_TO_STDOUT ) \
     || defined( PRJ_PRINTBITS_ENABLE_OUTPUT_TO_STDOUT )
 #include <stdio.h>
 #endif
 
 // ----------------------------------------------------------------------------------------
 
+#if defined( PRJ_PRINTDEC_ENABLE_OUTPUT_TO_BUFFER )
+
+char* print_decimal( char* outputbuf, uint8_t byte )
+{
+    outputbuf += 2;
+
+    *outputbuf-- = ( '0' + byte % 10 );
+    byte /= 10;
+
+    *outputbuf-- = ( '0' + byte % 10 );
+    byte /= 10;
+
+    *outputbuf = ( '0' + byte % 10 );
+
+    return outputbuf + 3;
+}
+
+#endif
+
+// ----------------------------------------------------------------------------------------
+
 #if defined( PRJ_PRINTHEX_ENABLE_OUTPUT_TO_STDOUT )
 
 void print_hex( uint8_t byte )
--- a/main/packages/common/util/printutils.h	Sat Jun 08 11:50:20 2013 -0700
+++ b/main/packages/common/util/printutils.h	Sat Jun 08 11:50:52 2013 -0700
@@ -39,6 +39,8 @@
 //
 //  enable various output styles:
 //
+//      + PRJ_PRINTDEC_ENABLE_OUTPUT_TO_BUFFER
+//
 //      + PRJ_PRINTHEX_ENABLE_OUTPUT_TO_STDOUT
 //      + PRJ_PRINTHEX_ENABLE_OUTPUT_TO_BUFFER
 //
@@ -48,6 +50,20 @@
 // ----------------------------------------------------------------------------------------
 
 // ----------------------------------------------------------------------------------------
+//  print_decimal
+//
+//      Print the decimal representation of the given value to the given buffer.
+//
+//      Bytes require 3 bytes in the output buffer, including any leading zeros. Words
+//      require 5 bytes, and long words require 10 bytes. There is no null terminator
+//      byte added.
+//
+//      The return value is a pointer within the buffer after the last byte added.
+//
+
+char* print_decimal( char* outputbuf, uint8_t byte );
+
+// ----------------------------------------------------------------------------------------
 //  print_hex
 //
 //      Print the hexidecimal representation of the given value to stdout.
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/canmsgs.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,153 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  CAN related functions for the Outdoor Robot motion control node.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include "project_defs.h"
+
+#include "func.h"
+
+#include "packages/avr/can/m1.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/avr/device/spinwait.h"
+
+// ----------------------------------------------------------------------------------------
+
+bool canmsg_init()
+{
+    return m1can_init();
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool canmsg_send_emergency()
+{
+    return m1can_send( can_build_message_id( can_node_odr_motion,
+                                             can_node_broadcast,
+                                             can_dataid_emergency ), 0, 0 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool canmsg_send_heartbeat()
+{
+    return m1can_send( can_build_message_id( can_node_odr_motion,
+                                             can_node_broadcast,
+                                             can_dataid_heartbeat ), 0, 0 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool canmsg_send_wheel_status()
+{
+    can_data_wheel_status data;
+
+    data.actual_rpm_front = encoder_wheel_rpm_front();
+    data.actual_rpm_rear  = wheel_speed_front(); //encoder_wheel_rpm_rear();
+
+    data.actual_motor_front = motorctl_speed_front();
+    data.actual_motor_rear  = motorctl_speed_rear();
+
+    uint32_t msgid = can_build_message_id( can_node_odr_motion,
+                                           can_node_broadcast,
+                                           can_dataid_wheel_status );
+
+    return m1can_send( msgid,
+                       reinterpret_cast< uint8_t* >( &data ),
+                       sizeof( data ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void update_servos( const can_data_servo_position* data )
+{
+    servo_set_positions( data->servo_front, data->servo_rear );
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void update_speed( const can_data_wheel_speed* data )
+{
+    wheel_speed_set( data->rpm_front, data->rpm_rear );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool canmsg_process_pending()
+{
+    uint32_t recvid;
+    bool     recvrequest = false;
+    uint8_t  recvdata[ 8 ];
+    uint8_t  recvlen = sizeof( recvdata );
+
+    if ( ! m1can_read( &recvid, &recvrequest, recvdata, &recvlen ) )
+    {
+        return true; // no messages, everything ok
+    }
+
+    uint8_t  srcnode;
+    uint8_t  dstnode;
+    uint16_t dataid;
+    can_parse_message_id( recvid, &srcnode, &dstnode, &dataid );
+
+    if ( dstnode != can_node_broadcast && dstnode != can_node_odr_motion )
+    {
+//        return true; // not for us, everything ok
+    }
+
+    switch ( dataid )
+    {
+        case can_dataid_emergency:
+            status_got_emergency( srcnode );
+            break;
+
+        case can_dataid_all_clear:
+            status_got_all_clear( srcnode );
+            break;
+
+        case can_dataid_heartbeat:
+            status_got_heartbeat( srcnode );
+            break;
+
+        case can_dataid_servo_position:
+            update_servos( reinterpret_cast< can_data_servo_position* >( &recvdata ) );
+            break;
+
+        case can_dataid_wheel_speed:
+            update_speed( reinterpret_cast< can_data_wheel_speed* >( &recvdata ) );
+            break;
+    }
+
+    return true; // no errors
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/encoders.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,198 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Motor encoder functions for the Outdoor Robot controller node.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include "project_defs.h"
+#include "func.h"
+
+#include "packages/avr/sensors/ls7366/ls7366.h"
+
+// ----------------------------------------------------------------------------------------
+//  The encoder from US Digital gives 120 ticks per revolution of the motor, prior to
+//  gear reduction. This robot uses E4P-120-079-HT.
+//
+//  The motor gives a reduction of 50:1 e.g. every 50 revolutions equals one revolution
+//  of the output shaft.
+//
+//  The differential gearbox between the motor output shaft and wheel contains 11 teeth
+//  on the shaft side, 34 teeth on the wheel side (reduction of 3.09:1).
+//  
+//  1 wheel revolution = 3.09 shaft revolutions = 154.5 motor revolutions = 18,540 ticks
+//  1 wheel RPM = 18,540 ticks per minute = 309 ticks per second
+//
+
+static const int32_t k_ticks_per_wheel_revolution = 18540L;
+static const int32_t k_ticks_per_wheel_revolution_per_second = 309L;
+
+static const int32_t k_min_ticks_for_one_rpm = 10;
+
+// ----------------------------------------------------------------------------------------
+
+void encoder_init()
+{
+    ls7366_init();
+}
+
+// ----------------------------------------------------------------------------------------
+
+void encoder_shutdown()
+{
+    // there is no shutdown routine needed
+}
+
+// ----------------------------------------------------------------------------------------
+
+static volatile int8_t g_wheel_rpm_front;
+static volatile int8_t g_wheel_rpm_rear;
+
+// ----------------------------------------------------------------------------------------
+
+static void refresh_front()
+{
+    static int32_t s_diffs_array[ 10 ];
+    static uint8_t s_index = 0;
+
+    static uint32_t s_previous_cntr = 0;
+
+    uint32_t cntr = ls7366_read_counter_1();
+
+    uint8_t dir;
+    int32_t diff = ls7366_compute_diff( s_previous_cntr, cntr, &dir );
+
+    if ( dir == 0 ) { diff *= -1; }
+
+    s_diffs_array[ s_index++ ] = diff;
+    s_index %= 10;
+
+    int32_t sum_of_diffs = 0;
+
+    for ( uint8_t i = 0; i < 10; ++i )
+    {
+        sum_of_diffs += s_diffs_array[ i ];
+    }
+
+    // this function is called 100 times per second, we add up the last ten values, so
+    // multiply the result by 10 to get RPM; to smooth the result, we scale the sum of
+    // values by 100
+
+    g_wheel_rpm_front = static_cast< int8_t >(
+                      ( sum_of_diffs * 100 )
+                      / ( k_ticks_per_wheel_revolution_per_second * 10 ) );
+
+    // special treatment to insure zero really means zero
+
+    if ( g_wheel_rpm_front == 0 )
+    {
+        if ( sum_of_diffs > k_min_ticks_for_one_rpm )
+        {
+            g_wheel_rpm_front = 1;
+        }
+        else if ( sum_of_diffs < -k_min_ticks_for_one_rpm )
+        {
+            g_wheel_rpm_front = -1;
+        }
+    }
+
+    s_previous_cntr = cntr;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void refresh_rear()
+{
+    static uint32_t s_diffs_array[ 10 ];
+    static uint8_t  s_index = 0;
+
+    static uint32_t s_previous_cntr = 0;
+
+    uint32_t cntr = ls7366_read_counter_2();
+
+    uint8_t dir;
+    int32_t diff = ls7366_compute_diff( s_previous_cntr, cntr, &dir );
+
+    if ( dir == 0 ) { diff *= -1; }
+
+    s_diffs_array[ s_index++ ] = diff;
+    s_index %= 10;
+
+    int32_t sum_of_diffs = 0;
+
+    for ( uint8_t i = 0; i < 10; ++i )
+    {
+        sum_of_diffs += s_diffs_array[ i ];
+    }
+
+    // this function is called 100 times per second, we add up the last ten values, so
+    // multiply the result by 10 to get RPM; to smooth the result, we scale the sum of
+    // values by 100
+
+    g_wheel_rpm_rear = static_cast< int8_t >(
+                     ( sum_of_diffs * 100 )
+                     / ( k_ticks_per_wheel_revolution_per_second * 10 ) );
+
+    // special treatment to insure zero really means zero
+
+    if ( g_wheel_rpm_rear == 0 )
+    {
+        if ( sum_of_diffs > k_min_ticks_for_one_rpm )
+        {
+            g_wheel_rpm_rear = 1;
+        }
+        else if ( sum_of_diffs < -k_min_ticks_for_one_rpm )
+        {
+            g_wheel_rpm_rear = -1;
+        }
+    }
+
+    s_previous_cntr = cntr;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void encoder_refresh()
+{
+    refresh_front();
+    refresh_rear();
+}
+
+// ----------------------------------------------------------------------------------------
+
+int8_t encoder_wheel_rpm_front()
+{
+    return g_wheel_rpm_front;
+}
+
+// ----------------------------------------------------------------------------------------
+
+int8_t encoder_wheel_rpm_rear()
+{
+    return g_wheel_rpm_rear;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/func.h	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,101 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012-2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Function prototypes.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#if !defined( FUNC_H )
+#define FUNC_H
+
+#include <stdint.h>
+
+// ----------------------------------------------------------------------------------------
+
+void status_init();
+void status_update();
+
+void status_got_emergency( uint8_t node );
+void status_got_all_clear( uint8_t node );
+void status_got_heartbeat( uint8_t node );
+
+bool status_is_emergency();
+
+// ----------------------------------------------------------------------------------------
+
+bool canmsg_init();
+bool canmsg_send_emergency();
+bool canmsg_send_heartbeat();
+bool canmsg_send_wheel_status();
+bool canmsg_process_pending();
+
+// ----------------------------------------------------------------------------------------
+
+void wheel_speed_refresh();
+void wheel_speed_set( int8_t front, int8_t rear );
+
+int8_t wheel_speed_front();
+int8_t wheel_speed_rear();
+
+// ----------------------------------------------------------------------------------------
+
+void servo_init();
+void servo_shutdown();
+
+void servo_set_positions( int8_t front, int8_t rear );
+
+int8_t servo_position_front();
+int8_t servo_position_rear();
+
+// ----------------------------------------------------------------------------------------
+
+void encoder_init();
+void encoder_shutdown();
+
+void encoder_refresh();
+
+int8_t encoder_wheel_rpm_front();
+int8_t encoder_wheel_rpm_rear();
+
+// ----------------------------------------------------------------------------------------
+
+bool motorctl_init();
+void motorctl_shutdown();
+
+bool motorctl_check_status();
+bool motorctl_is_in_error();
+
+bool motorctl_is_suspended();
+void motorctl_suspend();
+void motorctl_unsuspend();
+
+void motorctl_set_speed( int8_t front, int8_t rear );
+
+int8_t motorctl_speed_front();
+int8_t motorctl_speed_rear();
+
+// ----------------------------------------------------------------------------------------
+#endif // #if !defined( FUNC_H )
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/jamfile	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,52 @@
+# -----------------------------------------------------------------------------------------
+#
+#   Copyright (C) 2013 Bob Cook
+#
+#   Bob Cook Development, Robotics Library
+#   http://www.bobcookdev.com/rl/
+#    
+#   Permission is hereby granted, free of charge, to any person obtaining a copy
+#   of this software and associated documentation files (the "Software"), to deal
+#   in the Software without restriction, including without limitation the rights
+#   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+#   copies of the Software, and to permit persons to whom the Software is
+#   furnished to do so, subject to the following conditions:
+#
+#   The above copyright notice and this permission notice shall be included in
+#   all copies or substantial portions of the Software.
+#
+#   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+#   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+#   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+#   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+#   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+#   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+#   THE SOFTWARE.
+#
+# -----------------------------------------------------------------------------------------
+
+if $(TRACE) { echo "trace /robots/odr-motion/jamfile" ; }
+
+SubDir TOP robots odr-motion ;
+
+# -----------------------------------------------------------------------------------------
+
+avr_executable
+    odr-motion atmega16m1
+    : main.cpp
+      canmsgs.cpp
+      encoders.cpp
+      leds.cpp
+      motorctl.cpp
+      status.cpp servos.cpp
+      wheels.cpp
+      packages.avr.can.pkg
+      packages.avr.device.pkg
+      packages.avr.sensors.ls7366.pkg
+      packages.common.can.pkg
+      packages.common.pololu.pkg
+      packages.common.util.pkg
+    ;
+
+# -----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/leds.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,42 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  LED functions for the Outdoor Robot motion control board.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <avr/io.h>
+
+// ----------------------------------------------------------------------------------------
+
+void leds_off()
+{
+    PORTB |= ( 1 << PB2 );
+    PORTC |= ( 1 << PC4 ) | ( 1 << PC5 );
+    PORTD |= ( 1 << PD7 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/leds.h	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,67 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012-2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Function prototypes for the LEDs.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#if !defined( LEDS_H )
+#define LEDS_H
+
+// ----------------------------------------------------------------------------------------
+
+void leds_off();
+
+// ----------------------------------------------------------------------------------------
+
+void led_red_off() __attribute__((always_inline));
+void led_red_on() __attribute__((always_inline));
+void led_red_toggle() __attribute__((always_inline));
+
+// ----------------------------------------------------------------------------------------
+
+void led_yellow_off() __attribute__((always_inline));
+void led_yellow_on() __attribute__((always_inline));
+void led_yellow_toggle() __attribute__((always_inline));
+
+// ----------------------------------------------------------------------------------------
+
+void led_green_1_off() __attribute__((always_inline));
+void led_green_1_on() __attribute__((always_inline));
+void led_green_1_toggle() __attribute__((always_inline));
+
+// ----------------------------------------------------------------------------------------
+
+void led_green_2_off() __attribute__((always_inline));
+void led_green_2_on() __attribute__((always_inline));
+void led_green_2_toggle() __attribute__((always_inline));
+
+// ----------------------------------------------------------------------------------------
+
+#include "leds.inl"
+
+// ----------------------------------------------------------------------------------------
+#endif // #if !defined( LEDS_H )
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/leds.inl	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,111 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  LED functions for the Outdoor Robot motion control board.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <avr/io.h>
+
+//inline void led_red_off() __attribute__((always_inline));
+inline void led_red_off()
+{
+    PORTD |= ( 1 << PD7 );
+}
+
+//inline void led_red_on() __attribute__((always_inline));
+inline void led_red_on()
+{
+    PORTD &= ~( 1 << PD7 );
+}
+
+//inline void led_red_toggle() __attribute__((always_inline));
+inline void led_red_toggle()
+{
+    PORTD ^= ( 1 << PD7 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+inline void led_yellow_off() __attribute__((always_inline));
+inline void led_yellow_off()
+{
+    PORTB |= ( 1 << PB2 );
+}
+
+inline void led_yellow_on() __attribute__((always_inline));
+inline void led_yellow_on()
+{
+    PORTB &= ~( 1 << PB2 );
+}
+    
+inline void led_yellow_toggle() __attribute__((always_inline));
+inline void led_yellow_toggle()
+{
+    PORTB ^= ( 1 << PB2 );
+}
+    
+// ----------------------------------------------------------------------------------------
+
+inline void led_green_2_off() __attribute__((always_inline));
+inline void led_green_2_off()
+{
+    PORTC |= ( 1 << PC4 );
+}
+
+inline void led_green_2_on() __attribute__((always_inline));
+inline void led_green_2_on()
+{
+    PORTC &= ~( 1 << PC4 );
+}
+    
+inline void led_green_2_toggle() __attribute__((always_inline));
+inline void led_green_2_toggle()
+{
+    PORTC ^= ( 1 << PC4 );
+}
+    
+// ----------------------------------------------------------------------------------------
+
+inline void led_green_1_off() __attribute__((always_inline));
+inline void led_green_1_off()
+{
+    PORTC |= ( 1 << PC5 );
+}
+
+inline void led_green_1_on() __attribute__((always_inline));
+inline void led_green_1_on()
+{
+    PORTC &= ~( 1 << PC5 );
+}
+
+inline void led_green_1_toggle() __attribute__((always_inline));
+inline void led_green_1_toggle()
+{
+    PORTC ^= ( 1 << PC5 );
+}
+    
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/main.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,529 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Motion control node for the Outdoor Robot project, using a CAN interface.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include <avr/sleep.h>
+
+#include "project_defs.h"
+
+#include "func.h"
+#include "leds.h"
+
+#include "packages/avr/sensors/ls7366/ls7366.h"
+
+#include "packages/avr/device/int_helpers.h"
+#include "packages/avr/device/spinwait.h"
+
+#include "packages/common/util/misc.h"
+
+// always after other includes
+#include "packages/avr/device/workaround34734.h"
+
+// ----------------------------------------------------------------------------------------
+
+static const uint8_t trigger_main_loop_ok    = ( 1 << 0 );
+static const uint8_t trigger_send_heartbeat  = ( 1 << 1 );
+static const uint8_t trigger_update_encoder  = ( 1 << 2 );
+static const uint8_t trigger_adjust_wheels   = ( 1 << 3 );
+static const uint8_t trigger_check_motorctl  = ( 1 << 4 );
+static const uint8_t trigger_check_emergency = ( 1 << 5 );
+
+static volatile uint8_t g_triggers;
+
+// ----------------------------------------------------------------------------------------
+
+static const uint8_t odrmot_fatal_error_can_init = 1;
+static const uint8_t odrmot_fatal_error_can_comm = 2;
+static const uint8_t odrmot_fatal_error_exiting  = 9;
+
+void odrmot_fatal_error( uint8_t fault )
+{
+    cli(); // no more interrupts
+    DDRB = 0; DDRC = 0; DDRD = 0; // everything is an input
+    DDRB |= ( 1 << PB2 ); // except the leds
+    DDRC |= ( 1 << PC4 ) | ( 1 << PC5 );
+    DDRD |= ( 1 << PD7 );
+    leds_off();
+
+    for ( ;; )
+    {
+        for ( uint8_t i = 0; i < 5; i++ )
+        {
+            led_red_on();
+            spinwait_delay_ms( 125 );
+            led_red_off();
+            spinwait_delay_ms( 75 );
+        }
+
+        spinwait_delay_ms( 125 );
+        led_red_on();
+        led_yellow_on();
+        spinwait_delay_ms( 125 );
+
+        for ( uint8_t i = 0; i < fault; i++ )
+        {
+            led_green_1_on();
+            led_green_2_on();
+            spinwait_delay_ms( 350 );
+            
+            led_green_1_off();
+            led_green_2_off();
+            spinwait_delay_ms( 350 );
+        }
+
+        spinwait_delay_ms( 1000 );
+        
+        led_red_off();
+        led_yellow_off();
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void odrmot_hw_init()
+{
+    //--    Turn off all interrupts.
+
+    cli();
+    interrupts_clear_all();
+
+    //--    Indicator LEDs on PORTB.2, PORTC.4, PORTC.5, PORTD.7
+
+    DDRB |= ( 1 << PB2 );
+    DDRC |= ( 1 << PC4 ) | ( 1 << PC5 );
+    DDRD |= ( 1 << PD7 );
+    leds_off();
+
+    //--    Blink the LEDs just to say hello.
+    
+    for ( uint8_t i = 0; i < 5; i++ )
+    {
+        led_red_on();
+        led_yellow_on();
+        led_green_1_on();
+        led_green_2_on();
+        spinwait_delay_ms( 75 );
+        leds_off();
+        spinwait_delay_ms( 75 );
+    }
+
+    //--    Timer0 gives up a periodic "heartbeat" interrupt at ~100 Hz.
+
+    TCCR0A = 0
+
+        | ( 0 << COM0A1 ) | ( 0 << COM0A0 )                 // OC0A disconnected
+        | ( 0 << COM0B1 ) | ( 0 << COM0B0 )                 // OC0B disconnected
+        | ( 1 << WGM01  ) | ( 0 << WGM00  )                 // CTC mode
+        ;
+
+    TCCR0B = 0
+
+        | ( 0 << FOC0A ) | ( 0 << FOC0B )                   // no force output compare
+        | ( 0 << WGM02 )                                    // CTC mode
+        | ( 1 << CS02  ) | ( 0 << CS01  ) | ( 1 << CS00 )   // clk/1024
+        ;
+
+    OCR0A = 156;
+
+    TIMSK0 |= ( 1 << OCIE0A );
+
+    //--    Initialize the Pololu Qik 2s12v10 motor controller.
+
+    motorctl_init();
+
+    //--    Initialize the LS7366 chip(s).
+
+    DDRD |= ( 1 << PD3 ); // this is the SS pin, required to be an output for SPI master
+    ls7366_init();
+
+    //--    Initialize the CAN related functions and hardware.
+
+    if ( ! canmsg_init() )
+    {
+        odrmot_fatal_error( odrmot_fatal_error_can_init );
+    }
+
+    //--    When we sleep we want the "idle" mode e.g. wake up on any interrupt.
+
+    set_sleep_mode( SLEEP_MODE_IDLE );
+
+    //--    Re-enable interrupts.
+
+    sei();
+}
+
+// ----------------------------------------------------------------------------------------
+
+ISR( TIMER0_COMPA_vect )
+{
+    static uint8_t count = 0;
+
+    //--    The trigger_main_loop_ok flag indicates the main loop is "alive" e.g. still
+    //      running. We only blink the "heartbeat" LED if it continues to toggle.
+    //
+    // encoder update: every 10 milliseconds
+    // led blink pattern: 60 milliseconds on, 150 off, 60 on
+    // check ESTOP status: every 100 milliseconds
+    // adjust the motor speeds every 500 milliseconds
+    // check motor controller: every 1.25 seconds
+    // heartbeat: every 2.5 seconds
+    
+    g_triggers |= trigger_update_encoder;
+
+    if ( count % 10 == 0 )
+    {
+        g_triggers |= trigger_check_emergency;
+    }
+
+    if ( count % 50 == 0 )
+    {
+        g_triggers |= trigger_adjust_wheels;
+    }
+
+    switch ( ++count )
+    {
+        case 10:
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_on(); }
+            break;
+
+        case 16:
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_off(); }
+            break;
+
+        case 31:
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_on(); }
+            break;
+
+        case 37:
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_off(); }
+            g_triggers &= ~trigger_main_loop_ok;
+            break;
+
+        case 125:
+            g_triggers |= trigger_check_motorctl;
+            break;
+
+        case 250:
+            g_triggers |= ( trigger_send_heartbeat | trigger_check_motorctl );
+            count = 0;
+            break;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+//  The encoder from US Digital gives 120 ticks per revolution of the motor, prior to
+//  gear reduction. This robot uses E4P-120-079-HT.
+//
+//  The motor gives a reduction of 50:1 e.g. every 50 revolutions equals one revolution
+//  of the output shaft.
+//
+//  The differential gearbox between the motor output shaft and wheel contains 11 teeth
+//  on the shaft side, 34 teeth on the wheel side (reduction of 3.09:1).
+//  
+//  1 wheel revolution = 3.09 shaft revolutions = 154.5 motor revolutions = 18,540 ticks
+//  1 wheel RPM = 18,540 ticks per minute = 309 ticks per second
+//
+
+//const uint32_t k_ticks_per_wheel_revolution = 18540L;
+//const uint32_t k_ticks_per_wheel_revolution_per_second = 309L;
+
+// ----------------------------------------------------------------------------------------
+
+static volatile uint8_t g_wheel_rpm_setpt_front;
+static volatile uint8_t g_wheel_rpm_setpt_rear;
+
+#if 0
+void motorctl_set_speed( int8_t front, int8_t rear )
+{
+    if ( front < -23 )
+    {
+        g_dps_setpt_front = 230;
+    }
+    else if ( front < 0 )
+    {
+        g_dps_setpt_front = -front * 10;
+    }
+    else if ( front > 23 )
+    {
+        g_dps_setpt_front = 230;
+    }
+    else
+    {
+        g_dps_setpt_front = front * 10;
+    }
+}
+#endif
+
+// ----------------------------------------------------------------------------------------
+//  This function samples the motor encoders at 10 Hz, and stores the wheel rpm.
+//
+
+#if 0
+static volatile uint8_t g_wheel_rpm_front;
+static volatile uint8_t g_wheel_rpm_rear;
+
+void encoder_update_front()
+{
+    static uint32_t s_diffs_array[ 10 ];
+    static uint8_t  s_index = 0;
+
+    static uint32_t s_previous_cntr = 0;
+
+    uint32_t cntr = ls7366_read_counter_1();
+
+    uint8_t dir;
+    s_diffs_array[ s_index++ ] = ls7366_compute_diff( s_previous_cntr, cntr, &dir );
+    s_index %= 10;
+
+    uint32_t sum_of_diffs = 0;
+
+    for ( uint8_t i = 0; i < 10; ++i )
+    {
+        sum_of_diffs += s_diffs_array[ i ];
+    }
+
+    g_wheel_rpm_front = static_cast< uint8_t >(
+                      ( sum_of_diffs * 100 )
+                      / ( k_ticks_per_wheel_revolution_per_second * 100 ) );
+
+    s_previous_cntr = cntr;
+}
+
+void encoder_update_rear()
+{
+    static uint32_t s_diffs_array[ 10 ];
+    static uint8_t  s_index = 0;
+
+    static uint32_t s_previous_cntr = 0;
+
+    uint32_t cntr = ls7366_read_counter_2();
+
+    uint8_t dir;
+    s_diffs_array[ s_index++ ] = ls7366_compute_diff( s_previous_cntr, cntr, &dir );
+    s_index %= 10;
+
+    uint32_t sum_of_diffs = 0;
+
+    for ( uint8_t i = 0; i < 10; ++i )
+    {
+        sum_of_diffs += s_diffs_array[ i ];
+    }
+
+    g_wheel_rpm_rear = static_cast< uint8_t >(
+                     ( sum_of_diffs * 100 )
+                     / ( k_ticks_per_wheel_revolution_per_second * 100 ) );
+
+    s_previous_cntr = cntr;
+}
+#endif
+
+// ----------------------------------------------------------------------------------------
+
+static volatile int8_t g_motor_speed_front;
+static volatile int8_t g_motor_speed_rear;
+
+#if 0
+void adjust_motors()
+{
+    bool do_update = false;
+
+    if ( ( g_motor_dps + 1 ) < g_dps_setpt_front )
+    {
+        if ( g_motor_speed_front < 128 )
+        {
+            do_update = true;
+            ++g_motor_speed_front;
+        }
+    }
+    else if ( ( g_motor_dps - 1 ) > g_dps_setpt_front )
+    {
+        if ( g_motor_speed_front > 0 )
+        {
+            do_update = true;
+            --g_motor_speed_front;
+        }
+    }
+
+    if ( do_update )
+    {
+        send_motor_speed_msg( g_motor_speed_front, 0 );
+    }
+}
+#endif
+
+// ----------------------------------------------------------------------------------------
+
+int main()
+{
+    //--    Initialize the hardware and send a "hello we started" message.
+
+    odrmot_hw_init();
+
+    //--    Loop forever responding to CAN messages and system status.
+
+    uint8_t can_comm_errors = 0;
+
+    for ( ;; )
+    {
+        //--    The main loop is still running.
+
+        g_triggers |= trigger_main_loop_ok;
+
+        //--    Sleep until there is a new interrupt (the CAN driver gets one, as does
+        //      the periodic heartbeat, so we won't sleep forever).
+
+        sleep_mode();
+
+        //--    Time to update the encoder counters?
+
+        if ( g_triggers & trigger_update_encoder )
+        {
+            g_triggers &= ~trigger_update_encoder;
+
+            encoder_refresh();
+        }
+
+        //--    Adjust the wheel speeds?
+
+        if ( g_triggers & trigger_adjust_wheels )
+        {
+            g_triggers &= ~trigger_adjust_wheels;
+
+            wheel_speed_refresh();
+            canmsg_send_wheel_status();
+
+            if ( wheel_speed_front() != 0 )
+            {
+                led_green_2_on();
+            }
+            else
+            {
+                led_green_2_off();
+            }
+        }
+
+        //--    Check for an emergency situation?
+
+        if ( g_triggers & trigger_check_emergency )
+        {
+            g_triggers &= ~trigger_check_emergency;
+
+            if ( status_is_emergency() )
+            {
+//                led_red_on();
+                motorctl_suspend();
+                // suspend the servos
+            }
+            else
+            {
+//                led_red_off();
+                motorctl_unsuspend();
+                // unsuspend the servos
+            }
+        }
+
+#if 1
+        //--    Check the motor controller status?
+
+        if ( g_triggers & trigger_check_motorctl )
+        {
+            g_triggers &= ~trigger_check_motorctl;
+
+            if ( ! motorctl_check_status() )
+            {
+                led_red_on();
+            }
+            else
+            {
+                led_red_off();
+            }
+
+            if ( motorctl_is_in_error() )
+            {
+                motorctl_init();
+            }
+        }
+#endif
+
+        //--    Time to send the heartbeat message?
+
+        if ( g_triggers & trigger_send_heartbeat )
+        {
+            g_triggers &= ~trigger_send_heartbeat;
+
+            if ( ! canmsg_send_heartbeat() )
+            {
+                ++can_comm_errors;
+            }
+        }
+
+        //--    Any pending CAN messages to receive/process?
+
+        if ( ! canmsg_process_pending() )
+        {
+            ++can_comm_errors;
+        }
+        else
+        {
+            if ( can_comm_errors > 0 )
+            {
+                --can_comm_errors;
+            }
+        }
+
+        //--    Report warning and/or error conditions.
+
+        bool warning = false;
+
+        if ( can_comm_errors > 50 )
+        {
+            odrmot_fatal_error( odrmot_fatal_error_can_comm );
+        }
+        else if ( can_comm_errors > 10 )
+        {
+            warning = true;
+        }
+
+        if ( warning )
+        {
+            led_yellow_on();
+        }
+        else
+        {
+            led_yellow_off();
+        }
+    }
+
+    odrmot_fatal_error( odrmot_fatal_error_exiting );
+    return 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/motorctl.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,261 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012-2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Pololu Qik2s12v10 functions for the Outdoor Robot motion control node.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include <avr/pgmspace.h>
+
+#include <ctype.h>
+#include <string.h>
+
+#include "project_defs.h"
+#include "func.h"
+
+#include "packages/avr/device/spinwait.h"
+#include "packages/avr/device/uart.h"
+
+#include "packages/common/pololu/qik2s12v10.h"
+
+// ----------------------------------------------------------------------------------------
+
+static volatile bool   s_motor_error;
+static volatile bool   s_motor_suspended;
+
+static volatile int8_t s_motor_speed_front;
+static volatile int8_t s_motor_speed_rear;
+
+// ----------------------------------------------------------------------------------------
+
+inline void motorctl_qik_reset_high()
+{
+    PORTD |= ( 1 << PD6 );
+}
+
+inline void motorctl_qik_reset_low()
+{
+    PORTD &= ~( 1 << PD6 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool motorctl_init()
+{
+    //--    Qik motor controller reset on PORTD.6.
+    
+    DDRD |= ( 1 << PD6 );
+
+    //--    Qik motor controller error on PORTD.5. Turn off the pull-up.
+
+    DDRD &= ~( 1 << PD5 );
+    PORTD &= ~( 1 << PD5 );
+
+    //--    Initialize the Pololu Qik 2s12v10 motor controller.
+
+    motorctl_qik_reset_low();
+    spinwait_delay_ms( 100 );
+
+    motorctl_qik_reset_high();
+    spinwait_delay_ms( 100 );
+
+    if ( pololu_qik2s12v10_init() && ! motorctl_is_in_error() )
+    {
+        pololu_qik2s12v10_m0_forward( 0 );
+        pololu_qik2s12v10_m1_forward( 0 );
+        s_motor_error = false;
+        return true;
+    }
+
+    s_motor_error = true;
+    return false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void motorctl_shutdown()
+{
+    motorctl_qik_reset_low();
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool motorctl_check_status()
+{
+    uint8_t qik_firmware_version;
+    if ( ! pololu_qik2s12v10_firmware_version( &qik_firmware_version ) ) 
+    {
+        s_motor_error = true;
+        return false;
+    }
+
+    s_motor_error = false;
+    return true;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool motorctl_is_in_error()
+{
+    if ( ( PIND & ( 1 << PD5 ) ) > 0 )
+    {
+        return true;
+    }
+
+    if ( s_motor_error ) return true;
+
+    return false;
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool motorctl_is_suspended()
+{
+    return s_motor_suspended;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void motorctl_suspend()
+{
+    if ( ! s_motor_suspended )
+    {
+        s_motor_suspended = true;
+        pololu_qik2s12v10_m0_forward( 0 );
+        pololu_qik2s12v10_m1_forward( 0 );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void motorctl_unsuspend()
+{
+    if ( s_motor_suspended )
+    {
+        s_motor_suspended = false;
+
+        // update the front motor speed; yes forward is really reverse, see below
+        
+        if ( s_motor_speed_front < 0 )
+        {
+            pololu_qik2s12v10_m1_forward( -s_motor_speed_front );
+        }
+        else
+        {
+            pololu_qik2s12v10_m1_reverse( s_motor_speed_front );
+        }
+
+        // update the rear motor speed; ditto for reverse here too
+        
+        if ( s_motor_speed_rear < 0 )
+        {
+            pololu_qik2s12v10_m0_forward( -s_motor_speed_rear );
+        }
+        else
+        {
+            pololu_qik2s12v10_m0_reverse( s_motor_speed_rear );
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void set_speed_front( int8_t speed )
+{
+    if ( speed == s_motor_speed_front )
+    {
+        return;
+    }
+
+    s_motor_speed_front = speed;
+
+    if ( s_motor_suspended )
+    {
+        return;
+    }
+
+    // yes, the motor leads are backwards thus forward == reverse
+    
+    if ( speed < 0 )
+    {
+        pololu_qik2s12v10_m1_forward( -speed );
+    }
+    else
+    {
+        pololu_qik2s12v10_m1_reverse( speed );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void set_speed_rear( int8_t speed )
+{
+    if ( speed == s_motor_speed_rear )
+    {
+        return;
+    }
+
+    s_motor_speed_rear = speed;
+
+    if ( s_motor_suspended )
+    {
+        return;
+    }
+
+    // yes, the motor leads are backwards thus forward == reverse
+    
+    if ( speed < 0 )
+    {
+        pololu_qik2s12v10_m0_forward( -speed );
+    }
+    else
+    {
+        pololu_qik2s12v10_m0_reverse( speed );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void motorctl_set_speed( int8_t front, int8_t rear )
+{
+    set_speed_front( front );
+    set_speed_rear( rear );
+}
+
+// ----------------------------------------------------------------------------------------
+
+int8_t motorctl_speed_front()
+{
+    return s_motor_speed_front;
+}
+
+int8_t motorctl_speed_rear()
+{
+    return s_motor_speed_rear;
+}
+
+// ----------------------------------------------------------------------------------------
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/project_defs.h	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,80 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2013 Bob Cook
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+//  
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+//  
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#if !defined( PROJECT_DEFS_H )
+#define PROJECT_DEFS_H
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/common
+
+#define PRJ_CPU_FREQ    16000000L
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/can
+
+#define PRJ_M1CAN_ENABLE
+#define PRJ_M1CAN_CANBUS_250_KHZ
+#define PRJ_M1CAN_TX_BUFFER_SIZE  8
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/device
+
+#define PRJ_UART0_USE_POLLED_MODE
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/spi
+
+#define PRJ_SPI_ENABLE_MASTER
+#define PRJ_SPI_BUS_MODE_0
+#define PRJ_SPI_DATA_DIRECTION_MSB
+#define PRJ_SPI_CLOCK_FACTOR_DIV_16
+
+// ----------------------------------------------------------------------------------------
+//  packages/avr/sensors/ls7366
+
+#define PRJ_LS7366_ENABLE
+
+#define PRJ_LS7366_ENABLE_CHIP_1
+#define PRJ_LS7366_SELECT_1_DDR     DDRB
+#define PRJ_LS7366_SELECT_1_PORT    PORTB
+#define PRJ_LS7366_SELECT_1_PIN     PB6
+
+#define PRJ_LS7366_ENABLE_CHIP_2
+#define PRJ_LS7366_SELECT_2_DDR     DDRC
+#define PRJ_LS7366_SELECT_2_PORT    PORTC
+#define PRJ_LS7366_SELECT_2_PIN     PC0
+
+#define PRJ_LS7366_INC_COMPUTE_DIFF
+
+// ----------------------------------------------------------------------------------------
+//  packages/common/pololu/qik2s12v10
+
+#define PRJ_POLOLU_QIK2S12V10_ENABLE
+#define PRJ_POLOLU_QIK2S12V10_INPUT_AVR_UART0
+#define PRJ_POLOLU_QIK2S12V10_UART_BAUDRATE    9600
+
+
+#endif // #if !defined( PROJECT_DEFS_H )
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/servos.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,173 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2011-2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Servo management functions for the Outdoor Robot controller node.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <avr/interrupt.h>
+#include <avr/io.h>
+#include <avr/pgmspace.h>
+
+#include "func.h"
+
+#include "packages/avr/device/spinwait.h"
+
+#include "packages/common/util/misc.h"
+
+// always after other includes
+#include "packages/avr/device/workaround34734.h"
+
+// ----------------------------------------------------------------------------------------
+
+static volatile int8_t s_servo_position_front = 0;
+static volatile int8_t s_servo_position_rear  = 0;
+
+static void center_servos();
+
+// ----------------------------------------------------------------------------------------
+
+void servo_init()
+{
+    //--    Set up Timer1 to service the servo pulse interrupt. We want a 50 Hz cycle for
+    //      the primary PWM cycle and we'll use OCR1n to drive each servo's logic pin.
+
+    TCCR1A = 0
+
+        | ( 1 << COM1A1 ) | ( 1 << COM1A0 )     // set OC1A on up, clear on down
+        | ( 1 << COM1B1 ) | ( 1 << COM1B0 )     // set OC1B on up, clear on down
+        | ( 1 << WGM11  ) | ( 0 << WGM10  )     // PWM phase correct (mode 10)
+        ;
+
+    TCCR1B = 0
+
+        | ( 0 << ICNC1 )                                    // no input capture
+        | ( 0 << ICES1 )                                    // no input capture
+        | ( 1 << WGM13 ) | ( 0 << WGM12 )                   // PWM phase correct (mode 10)
+        | ( 0 << CS12  ) | ( 1 << CS11 ) | ( 0 << CS10 )    // clk/8
+        ;
+
+    TCCR1C = 0
+
+        | ( 0 << FOC1A )    // no force output compare
+        | ( 0 << FOC1B )    // no force output compare
+        ;
+
+    ICR1 = 20000; // 10ms counting up, 10ms counting down (for 16Mhz clock)
+
+    DDRC |= ( 1 << PC1 );   // front servo is on PC1
+    DDRD |= ( 1 << PD2 );   // rear servo is on PD2
+   
+    center_servos();    // initialize to center positions 
+}
+
+// ----------------------------------------------------------------------------------------
+
+void servo_shutdown()
+{
+    TCCR1A = 0; // Timer1 disabled
+}
+
+// ----------------------------------------------------------------------------------------
+
+// experimentally determined range: [18824, 17920]
+static const uint16_t PROGMEM g_front_servo_table[] = {
+    18824, 18800, 18776, 18748, 18720, 18688, 18656, 18624, 18592, 18560, 18528, 18472,
+    18416, // center
+    18356, 18296, 18260, 18224, 18184, 18144, 18108, 18072, 18032, 17992, 17956, 17920
+};
+
+// experimentally determined range: [18680, 17880]
+static const uint16_t PROGMEM g_rear_servo_table[] = {
+    18680, 18656, 18632, 18604, 18576, 18544, 18512, 18480, 18448, 18412, 18376, 18332,
+    18288, // center
+    18260, 18232, 18192, 18152, 18116, 18080, 18044, 18008, 17976, 17944, 17912, 17880
+};
+
+static void center_servos()
+{
+    s_servo_position_front = 0;
+    OCR1A = pgm_read_word( g_front_servo_table + s_servo_position_front + 12 );
+
+    s_servo_position_rear  = 0;
+    OCR1B = pgm_read_word( g_rear_servo_table  + s_servo_position_rear  + 12 );
+}
+
+void servo_set_positions( int8_t front, int8_t rear )
+{
+    if ( front == s_servo_position_front && rear == s_servo_position_rear )
+    {
+        return;
+    }
+
+    int8_t f = util_clamp( static_cast< int8_t >( -12 ),
+                           front,
+                           static_cast< int8_t >( 12 ) );
+
+    int8_t r = util_clamp( static_cast< int8_t >( -12 ),
+                           rear,
+                           static_cast< int8_t >( 12 ) );
+
+    while ( f != s_servo_position_front || r != s_servo_position_rear )
+    {
+        OCR1A = pgm_read_word( g_front_servo_table + s_servo_position_front + 12 );
+        OCR1B = pgm_read_word( g_rear_servo_table  + s_servo_position_rear  + 12 );
+
+        if ( s_servo_position_front > f )
+        {
+            --s_servo_position_front;
+        }
+        else if ( s_servo_position_front < f )
+        {
+            ++s_servo_position_front;
+        }
+
+        if ( s_servo_position_rear > r )
+        {
+            --s_servo_position_rear;
+        }
+        else if ( s_servo_position_rear < r )
+        {
+            ++s_servo_position_rear;
+        }
+
+        spinwait_delay_ms( 10 );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+int8_t servo_position_front()
+{
+    return s_servo_position_front;
+}
+
+int8_t servo_position_rear()
+{
+    return s_servo_position_rear;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/status.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,115 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2011 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Status display functions for the Outdoor Robot controller node.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+//  There is an implicit assumption our "heartbeat" is 2 Hz (twice per second).
+//
+// ----------------------------------------------------------------------------------------
+
+#include "func.h"
+#include "leds.h"
+
+#include <avr/io.h>
+
+#include "packages/common/can/can_nodes.h"
+
+// ----------------------------------------------------------------------------------------
+//  "beats" is 500ms (approximately)
+
+static volatile uint8_t g_beats_without_mgr;
+
+static volatile uint8_t g_status_flags;
+
+static const uint8_t status_flag_emergency_active = ( 1 << 0 );
+
+// ----------------------------------------------------------------------------------------
+
+void status_init()
+{
+    // initalize the heartbeat watchdog assuming no contact yet
+    g_beats_without_mgr = 100;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_update()
+{
+    if ( g_beats_without_mgr < 200 ) { ++g_beats_without_mgr; }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_got_heartbeat( uint8_t source )
+{
+    led_red_toggle();
+
+    switch ( source )
+    {
+        case can_node_odr_manager:
+            g_beats_without_mgr = 0;
+            break;
+
+//        case can_node_odr_sonar_front:
+//            break;
+
+//        case can_node_gps:
+//            g_beats_without_gps = 0;
+//            break;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_got_emergency( uint8_t source )
+{
+    g_status_flags |= status_flag_emergency_active;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_got_all_clear( uint8_t source )
+{
+    if ( source == can_node_odr_manager )
+    {
+        g_status_flags &= ~status_flag_emergency_active;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool status_is_emergency()
+{
+    // more than two seconds == down
+    if ( g_beats_without_mgr > 4 )
+    {
+        return true;
+    }
+
+    return ( g_status_flags & status_flag_emergency_active ) > 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/wheels.cpp	Sat Jun 08 11:50:52 2013 -0700
@@ -0,0 +1,174 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Wheel speed and position functions for the Outdoor Robot motion control node.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+//#include <ctype.h>
+//#include <string.h>
+
+#include "project_defs.h"
+#include "func.h"
+
+// ----------------------------------------------------------------------------------------
+
+static const int8_t k_max_motor_error_constant = 20;
+static const int8_t k_error_gain = 3;
+
+static volatile int8_t s_wheel_rpm_front;
+static volatile int8_t s_motor_front;
+
+static volatile int8_t s_wheel_rpm_rear;
+static volatile int8_t s_motor_rear;
+
+// ----------------------------------------------------------------------------------------
+
+void wheel_init()
+{
+    s_wheel_rpm_front = 0;
+    s_motor_front = 0;
+    s_wheel_rpm_rear = 0;
+    s_motor_rear = 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void adjust_motor_front( int8_t delta )
+{
+    int16_t m = s_motor_front;
+    m += delta;
+
+    if ( m < -127 )
+    {
+        s_motor_front = -127;
+    }
+    else if ( m > 127 )
+    {
+        s_motor_front = 127;
+    }
+    else
+    {
+        s_motor_front = static_cast< int8_t >( m );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void adjust_motor_rear( int8_t delta )
+{
+    int16_t m = s_motor_rear;
+    m += delta;
+
+    if ( m < -127 )
+    {
+        s_motor_rear = -127;
+    }
+    else if ( m > 127 )
+    {
+        s_motor_rear = 127;
+    }
+    else
+    {
+        s_motor_rear = static_cast< int8_t >( m );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void wheel_speed_refresh()
+{
+    int8_t error_front = s_wheel_rpm_front - encoder_wheel_rpm_front();
+
+    if ( error_front > 1 )
+    {
+        error_front *= k_error_gain;
+    }
+
+    if ( error_front > k_max_motor_error_constant )
+    {
+        adjust_motor_front( k_max_motor_error_constant );
+    }
+    else if ( error_front < -k_max_motor_error_constant )
+    {
+        adjust_motor_front( -k_max_motor_error_constant );
+    }
+    else
+    {
+        adjust_motor_front( error_front );
+    }
+
+    int8_t error_rear = s_wheel_rpm_rear - encoder_wheel_rpm_rear();
+
+    if ( error_rear > 1 )
+    {
+        error_rear *= k_error_gain;
+    }
+
+    if ( error_rear > k_max_motor_error_constant )
+    {
+        adjust_motor_rear( k_max_motor_error_constant );
+    }
+    else if ( error_rear < -k_max_motor_error_constant )
+    {
+        adjust_motor_rear( k_max_motor_error_constant );
+    }
+    else
+    {
+        adjust_motor_rear( error_rear );
+    }
+
+    if ( error_front != 0 || error_rear != 0 )
+    {
+        motorctl_set_speed( s_motor_front, s_motor_rear );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void wheel_speed_set( int8_t front, int8_t rear )
+{
+    s_wheel_rpm_front = front;
+    s_wheel_rpm_rear  = rear;
+
+    wheel_speed_refresh();
+}
+
+// ----------------------------------------------------------------------------------------
+
+int8_t wheel_speed_front()
+{
+    return s_wheel_rpm_front;
+}
+
+// ----------------------------------------------------------------------------------------
+
+int8_t wheel_speed_rear()
+{
+    return s_wheel_rpm_rear;
+}
+
+// ----------------------------------------------------------------------------------------
+