changeset 196:aa2665165208 main

Updated display and emergency situation handling.
author Bob Cook <bob@bobcookdev.com>
date Wed, 21 Aug 2013 21:02:10 -0700
parents d6621e43b91b
children ff6cfa2ea1ee
files main/robots/odr-controller/canmsgs.cpp main/robots/odr-controller/display.cpp main/robots/odr-controller/func.h main/robots/odr-controller/jamfile main/robots/odr-controller/leds.cpp main/robots/odr-controller/leds.h main/robots/odr-controller/leds.inl main/robots/odr-controller/main.cpp main/robots/odr-controller/motorctl.cpp main/robots/odr-controller/project_defs.h main/robots/odr-controller/servos.cpp main/robots/odr-controller/status.cpp
diffstat 12 files changed, 598 insertions(+), 718 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-controller/canmsgs.cpp	Wed Aug 21 21:01:39 2013 -0700
+++ b/main/robots/odr-controller/canmsgs.cpp	Wed Aug 21 21:02:10 2013 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2011 Bob Cook
+//  Copyright (C) 2011-2013 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
@@ -71,23 +71,29 @@
 
 // ----------------------------------------------------------------------------------------
 
-bool canmsg_send_update()
+bool canmsg_send_emergency()
 {
-    uint32_t msgid = can_build_message_id( can_node_odr_controller,
-                                           can_node_broadcast,
-                                           can_dataid_odrctl_update );
+    return mcp2515_send( can_build_message_id( can_node_odr_controller,
+                                               can_node_broadcast,
+                                               can_dataid_emergency ), 0, 0 );
+}
 
-    can_data_odrctl_update info = { 0 };
+// ----------------------------------------------------------------------------------------
 
-    info.estop    = status_is_estop_active();
-    info.motorctl = motorctl_check_status();
+bool canmsg_send_all_clear()
+{
+    return mcp2515_send( can_build_message_id( can_node_odr_controller,
+                                               can_node_broadcast,
+                                               can_dataid_all_clear ), 0, 0 );
+}
 
-    if ( ! mcp2515_send( msgid, reinterpret_cast< uint8_t* >( &info ), sizeof( info ) ) )
-    {
-        return false;
-    }
-    
-    return true;
+// ----------------------------------------------------------------------------------------
+
+bool canmsg_send_heartbeat()
+{
+    return mcp2515_send( can_build_message_id( can_node_odr_controller,
+                                               can_node_broadcast,
+                                               can_dataid_heartbeat ), 0, 0 );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -113,17 +119,10 @@
 
 // ----------------------------------------------------------------------------------------
 
-static void update_servos( const can_data_servo_position* data )
+static void update_wheel_status( const can_data_wheel_status* data )
 {
-    servo_set_positions( data->servo_front, data->servo_rear );
-}
-
-// ----------------------------------------------------------------------------------------
-
-static void update_motors( const can_data_motor_speed* data )
-{
-    motorctl_set_speed_front( data->motor_front );
-    motorctl_set_speed_rear( data->motor_rear );
+    display_update_wheel_speed( data->actual_rpm_front, data->actual_rpm_rear );
+    display_update_wheel_position( data->angle_front, data->angle_rear );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -199,17 +198,21 @@
             status_got_heartbeat( srcnode );
             break;
 
+        case can_dataid_emergency:
+            status_got_emergency( srcnode );
+            break;
+
+        case can_dataid_all_clear:
+            status_got_all_clear( srcnode );
+            break;
+
         case can_dataid_odrmgr_update:
             got_mgr_update(
                     reinterpret_cast< can_data_odrmgr_update* >( &recvdata ), recvlen );
             break;
 
-        case can_dataid_servo_position:
-            update_servos( reinterpret_cast< can_data_servo_position* >( &recvdata ) );
-            break;
-
-        case can_dataid_motor_speed:
-            update_motors( reinterpret_cast< can_data_motor_speed* >( &recvdata ) );
+        case can_dataid_wheel_status:
+            update_wheel_status( reinterpret_cast< can_data_wheel_status* >( &recvdata ) );
             break;
 
         case can_dataid_imu_yaw:
--- a/main/robots/odr-controller/display.cpp	Wed Aug 21 21:01:39 2013 -0700
+++ b/main/robots/odr-controller/display.cpp	Wed Aug 21 21:02:10 2013 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2012 Bob Cook
+//  Copyright (C) 2012-2013 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
@@ -50,13 +50,14 @@
 //      1-|                     |-
 //      2-|      XXXXXXXX       |-          X = status message from mgr
 //      3-|                     |-
-//      4-|motor f:+000  r:+000 |-
-//      5-|servo f:+000  r:+000 |-
+//      4-|  motion controller  |-
+//      5-| f:+00/+00 r:+00/+00 |-
 //      6-|                     |-
-//      7-|     front sonar     |-
-//      8-| 0000 0000 0000 0000 |-
+//      7-|  front rangefinder  |-
+//      8-|   000 000 000 000   |-
 //      9-|                     |-
 //     10-| Y+000 P+00.0 R+00.0 |-
+//      1-| Target:+000 (+000)  |-
 //      1-|                     |-
 //      2-|                     |-
 //      3-|                     |-
@@ -78,6 +79,7 @@
 
 static void str_small_int( char* str, int8_t value )
 {
+/*
     if ( value >= 100 )
     {
         *str++ = '1';
@@ -87,6 +89,7 @@
     {
         *str++ = '0';
     }
+*/
 
     if ( value >= 10 )
     {
@@ -118,66 +121,22 @@
     }
     else
     {
-        *str++ = ' ';
+        *str++ = '0';
     }
 
     str_small_int( str, value );
 }
 
 // ----------------------------------------------------------------------------------------
-//  012345678901234567890
-//  motor f:sxxx  r:sxxx  
-
-static void update_motor_info()
-{
-    if ( ! motorctl_check_status() )
-    {
-        sfe569_write_pgm( PSTR("err!"),  8, 4, sfe569_color_white, sfe569_color_red );
-        sfe569_write_pgm( PSTR("err!"), 16, 4, sfe569_color_white, sfe569_color_red );
-        return;
-    }
-
-    uint8_t background = sfe569_color_blue;
-
-    if ( motorctl_is_suspended() )
-    {
-        background = sfe569_color_red;
-    }
-
-    char buf[ 6 ];
-
-    str_small_int_with_sign( buf, motorctl_speed_front() );
-    sfe569_write( buf, 8, 4, sfe569_color_white, background );
-
-    str_small_int_with_sign( buf, motorctl_speed_rear() );
-    sfe569_write( buf, 16, 4, sfe569_color_white, background );
-}
-
-// ----------------------------------------------------------------------------------------
-//  012345678901234567890
-//  servo f:sxxx  r:sxxx  
-
-static void update_servo_info()
-{
-    char buf[ 6 ];
-
-    str_small_int_with_sign( buf, servo_position_front() );
-    sfe569_write( buf, 8, 5, sfe569_color_white, sfe569_color_blue );
-
-    str_small_int_with_sign( buf, servo_position_rear() );
-    sfe569_write( buf, 16, 5, sfe569_color_white, sfe569_color_blue );
-}
-
-// ----------------------------------------------------------------------------------------
 
 void display_init()
 {
     sfe569_init();
     sfe569_fill_screen( sfe569_color_blue );
     sfe569_write_pgm( PSTR("Outdoor Robot"), 0, 0, sfe569_color_white, sfe569_color_blue );
-    sfe569_write_pgm( PSTR("motor f:      r:"), 0, 4, sfe569_color_white, sfe569_color_blue );
-    sfe569_write_pgm( PSTR("servo f:      r:"), 0, 5, sfe569_color_white, sfe569_color_blue );
-    sfe569_write_pgm( PSTR("front sonar"), 5, 7, sfe569_color_white, sfe569_color_blue );
+    sfe569_write_pgm( PSTR("motion controller"), 2, 4, sfe569_color_white, sfe569_color_blue );
+    sfe569_write_pgm( PSTR("f:   /    r:   /"), 1, 5, sfe569_color_white, sfe569_color_blue );
+    sfe569_write_pgm( PSTR("front rangefinder"), 2, 7, sfe569_color_white, sfe569_color_blue );
     sfe569_write_pgm( PSTR("Y---- P----- R-----"), 1, 10, sfe569_color_white, sfe569_color_blue );
 }
 
@@ -236,6 +195,27 @@
 
 // ----------------------------------------------------------------------------------------
 
+static void update_wheel_info()
+{
+    if ( ! status_is_motion_ctl_alive() )
+    {
+        sfe569_write_pgm( PSTR("      offline      "),
+                          1, 5, sfe569_color_white, sfe569_color_red );
+    }
+    else
+    {
+        sfe569_write( 'f',  1, 5, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ':',  2, 5, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( '/',  6, 5, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ' ', 10, 5, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( 'r', 11, 5, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ':', 12, 5, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( '/', 16, 5, sfe569_color_white, sfe569_color_blue );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
 static void update_sonar_front_info()
 {
     if ( ! status_is_sonar_front_up() )
@@ -248,6 +228,16 @@
         sfe569_write_pgm( PSTR("      disabled     "),
                           1, 8, sfe569_color_white, sfe569_color_blue );
     }
+    else
+    {
+        sfe569_write( ' ',  1, 8, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ' ',  2, 8, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ' ',  6, 8, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ' ', 10, 8, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ' ', 14, 8, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ' ', 18, 8, sfe569_color_white, sfe569_color_blue );
+        sfe569_write( ' ', 19, 8, sfe569_color_white, sfe569_color_blue );
+    }
 }
 
 // ----------------------------------------------------------------------------------------
@@ -256,8 +246,7 @@
 {
     update_heartbeat_flag();
     update_estop_flag();
-    update_motor_info();
-    update_servo_info();
+    update_wheel_info();
     update_sonar_front_info();
 
     ++g_spinner_count;
@@ -316,36 +305,58 @@
 
 // ----------------------------------------------------------------------------------------
 
+void display_update_wheel_speed( int8_t front, int8_t rear )
+{
+    char buf[ 6 ];
+
+    str_small_int_with_sign( buf, front );
+    sfe569_write( buf, 3, 5, sfe569_color_white, sfe569_color_blue );
+
+    str_small_int_with_sign( buf, rear );
+    sfe569_write( buf, 13, 5, sfe569_color_white, sfe569_color_blue );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void display_update_wheel_position( int8_t front, int8_t rear )
+{
+    char buf[ 6 ];
+
+    str_small_int_with_sign( buf, front );
+    sfe569_write( buf, 7, 5, sfe569_color_white, sfe569_color_blue );
+
+    str_small_int_with_sign( buf, rear );
+    sfe569_write( buf, 17, 5, sfe569_color_white, sfe569_color_blue );
+}
+
+// ----------------------------------------------------------------------------------------
+
 void display_update_sonar_front
 (
-    uint16_t left,
-    uint16_t center_left,
-    uint16_t center_right,
-    uint16_t right
+    uint8_t left,
+    uint8_t center_left,
+    uint8_t center_right,
+    uint8_t right
 )
 {
     char  buf[ 6 ];
     char* p = buf;
 
-    sfe569_write( ' ',  5, 8, sfe569_color_white, sfe569_color_blue );
-    sfe569_write( ' ', 10, 8, sfe569_color_white, sfe569_color_blue );
-    sfe569_write( ' ', 15, 8, sfe569_color_white, sfe569_color_blue );
-
-    p = print_hex( buf, left );
+    p = print_decimal( buf, left );
     *p = '\0';
-    sfe569_write( buf, 1, 8, sfe569_color_white, sfe569_color_blue );
+    sfe569_write( buf, 3, 8, sfe569_color_white, sfe569_color_blue );
 
-    p = print_hex( buf, center_left );
+    p = print_decimal( buf, center_left );
     *p = '\0';
-    sfe569_write( buf, 6, 8, sfe569_color_white, sfe569_color_blue );
+    sfe569_write( buf, 7, 8, sfe569_color_white, sfe569_color_blue );
 
-    p = print_hex( buf, center_right );
+    p = print_decimal( buf, center_right );
     *p = '\0';
     sfe569_write( buf, 11, 8, sfe569_color_white, sfe569_color_blue );
 
-    p = print_hex( buf, right );
+    p = print_decimal( buf, right );
     *p = '\0';
-    sfe569_write( buf, 16, 8, sfe569_color_white, sfe569_color_blue );
+    sfe569_write( buf, 15, 8, sfe569_color_white, sfe569_color_blue );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -434,4 +445,28 @@
 }
 
 // ----------------------------------------------------------------------------------------
+//      1-| Target:+000 (+000)  |-
 
+void display_update_nav_info( int16_t target, int16_t error )
+{
+#if 0
+    char buf[ 8 ];
+
+    // target is displayed as a whole integer, dropping the fractional portion
+
+    format_imu_value( target, buf );
+    buf[ 4 ] = '\0';
+
+    sfe569_write( buf, 8, 11, sfe569_color_white, sfe569_color_blue );
+
+    // error is displayed as a whole signed value
+
+    format_imu_value( error, buf );
+    buf[ 4 ] = '\0';
+
+    sfe569_write( buf, 8, 11, sfe569_color_white, sfe569_color_blue );
+#endif
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-controller/func.h	Wed Aug 21 21:01:39 2013 -0700
+++ b/main/robots/odr-controller/func.h	Wed Aug 21 21:02:10 2013 -0700
@@ -38,13 +38,17 @@
 void status_update();
 
 void status_got_heartbeat( uint8_t node );
+void status_got_emergency( uint8_t node );
+void status_got_all_clear( uint8_t node );
 void status_got_mgr_update();
 void status_got_sonar_front_enabled();
 void status_got_sonar_front_disabled();
 void status_got_rf_message( const uint8_t* data, uint8_t length );
 
+bool status_is_emergency();
 bool status_is_mgr_up();
 bool status_is_estop_active();
+bool status_is_motion_ctl_alive();
 bool status_is_sonar_front_up();
 bool status_is_sonar_front_enabled();
 
@@ -56,7 +60,10 @@
 void display_set_mgr_status( const char* msg, uint8_t length );
 void display_clear_mgr_status();
 
-void display_update_sonar_front( uint16_t l, uint16_t c_l, uint16_t c_r, uint16_t r );
+void display_update_wheel_speed( int8_t front, int8_t rear );
+void display_update_wheel_position( int8_t front, int8_t rear );
+
+void display_update_sonar_front( uint8_t l, uint8_t c_l, uint8_t c_r, uint8_t r );
 
 void display_update_yaw( int32_t value );
 void display_update_pitch( int32_t value );
@@ -68,40 +75,14 @@
 // ----------------------------------------------------------------------------------------
 
 bool canmsg_init();
-bool canmsg_send_update();
+bool canmsg_send_emergency();
+bool canmsg_send_all_clear();
+bool canmsg_send_heartbeat();
 bool canmsg_send_button_status();
 bool canmsg_process_pending();
 
 // ----------------------------------------------------------------------------------------
 
-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();
-
-// ----------------------------------------------------------------------------------------
-
-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_front( int8_t speed );
-void motorctl_set_speed_rear( int8_t speed );
-
-int8_t motorctl_speed_front();
-int8_t motorctl_speed_rear();
-
-// ----------------------------------------------------------------------------------------
-
 bool button_is_1_down();
 bool button_is_2_down();
 
--- a/main/robots/odr-controller/jamfile	Wed Aug 21 21:01:39 2013 -0700
+++ b/main/robots/odr-controller/jamfile	Wed Aug 21 21:02:10 2013 -0700
@@ -33,7 +33,7 @@
 
 avr_executable
     odr-controller atmega128
-    : main.cpp canmsgs.cpp display.cpp motorctl.cpp servos.cpp status.cpp
+    : main.cpp canmsgs.cpp display.cpp leds.cpp status.cpp
       packages.avr.can.pkg
       packages.avr.device.pkg
       packages.avr.lcd.sfe569.pkg
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-controller/leds.cpp	Wed Aug 21 21:02:10 2013 -0700
@@ -0,0 +1,40 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  LED functions for the Outdoor Robot display module 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()
+{
+    PORTF |= ( 1 << PF4 ) | ( 1 << PF5 ) | ( 1 << PF6 ) | ( 1 << PF7 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-controller/leds.h	Wed Aug 21 21:02:10 2013 -0700
@@ -0,0 +1,72 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2012-2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  LED functions for the Outdoor Robot display module 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.
+//
+// ----------------------------------------------------------------------------------------
+
+#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));
+
+// ----------------------------------------------------------------------------------------
+
+void led_estop_override_off() __attribute__((always_inline));
+void led_estop_override_on() __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-controller/leds.inl	Wed Aug 21 21:02:10 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 display module 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()
+{
+    PORTF |= ( 1 << PF7 );
+}
+
+inline void led_red_on()
+{
+    PORTF &= ~( 1 << PF7 );
+}
+
+inline void led_red_toggle()
+{
+    PORTF ^= ( 1 << PF7 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+inline void led_yellow_off()
+{
+    PORTF |= ( 1 << PF6 );
+}
+
+inline void led_yellow_on()
+{
+    PORTF &= ~( 1 << PF6 );
+}
+    
+inline void led_yellow_toggle()
+{
+    PORTF ^= ( 1 << PF6 );
+}
+    
+// ----------------------------------------------------------------------------------------
+
+inline void led_green_2_off()
+{
+    PORTF |= ( 1 << PF5 );
+}
+
+inline void led_green_2_on()
+{
+    PORTF &= ~( 1 << PF5 );
+}
+    
+inline void led_green_2_toggle()
+{
+    PORTF ^= ( 1 << PF5 );
+}
+    
+// ----------------------------------------------------------------------------------------
+
+inline void led_green_1_off()
+{
+    PORTF |= ( 1 << PF4 );
+}
+
+inline void led_green_1_on()
+{
+    PORTF &= ~( 1 << PF4 );
+}
+
+inline void led_green_1_toggle()
+{
+    PORTF ^= ( 1 << PF4 );
+}
+    
+// ----------------------------------------------------------------------------------------
+
+inline void led_estop_override_off()
+{
+    PORTA &= ~( 1 << PA3 );
+}
+
+inline void led_estop_override_on()
+{
+    PORTA |= ( 1 << PA3 );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-controller/main.cpp	Wed Aug 21 21:01:39 2013 -0700
+++ b/main/robots/odr-controller/main.cpp	Wed Aug 21 21:02:10 2013 -0700
@@ -1,11 +1,11 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2012 Bob Cook
+//  Copyright (C) 2012-2013 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
 // 
-//  Controller for the Outdoor Robot project, using a CAN interface.
+//  Display module 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
@@ -33,6 +33,7 @@
 
 #include "project_defs.h"
 #include "func.h"
+#include "leds.h"
 
 #include "packages/avr/device/int_helpers.h"
 #include "packages/avr/device/simavr_helpers.h"
@@ -49,11 +50,12 @@
 
 // ----------------------------------------------------------------------------------------
 
-static const uint8_t trigger_update_status  = ( 1 << 0 );
-static const uint8_t trigger_update_display = ( 1 << 1 );
-static const uint8_t trigger_button_event   = ( 1 << 2 );
-static const uint8_t trigger_check_motorctl = ( 1 << 3 );
-static const uint8_t trigger_send_heartbeat = ( 1 << 4 );
+static const uint8_t trigger_main_loop_ok    = ( 1 << 0 );
+static const uint8_t trigger_update_status   = ( 1 << 1 );
+static const uint8_t trigger_update_display  = ( 1 << 2 );
+static const uint8_t trigger_button_event    = ( 1 << 3 );
+static const uint8_t trigger_send_heartbeat  = ( 1 << 4 );
+static const uint8_t trigger_check_emergency = ( 1 << 5 );
 
 static volatile uint8_t g_triggers;
 
@@ -64,84 +66,8 @@
 
 // ----------------------------------------------------------------------------------------
 
-inline void leds_off()
-{
-    PORTF |= ( 1 << PF4 ) | ( 1 << PF5 ) | ( 1 << PF6 ) | ( 1 << PF7 );
-}
-
-// ----------------------------------------------------------------------------------------
-
-inline void led_red_off()
-{
-    PORTF |= ( 1 << PF7 );
-}
-
-inline void led_red_on()
-{
-    PORTF &= ~( 1 << PF7 );
-}
-
-inline void led_red_toggle()
-{
-    PORTF ^= ( 1 << PF7 );
-}
-
-// ----------------------------------------------------------------------------------------
-
-inline void led_yellow_off()
-{
-    PORTF |= ( 1 << PF6 );
-}
-
-inline void led_yellow_on()
-{
-    PORTF &= ~( 1 << PF6 );
-}
-    
-inline void led_yellow_toggle()
-{
-    PORTF ^= ( 1 << PF6 );
-}
-    
-// ----------------------------------------------------------------------------------------
-
-inline void led_green_2_off()
-{
-    PORTF |= ( 1 << PF5 );
-}
-
-inline void led_green_2_on()
-{
-    PORTF &= ~( 1 << PF5 );
-}
-    
-inline void led_green_2_toggle()
-{
-    PORTF ^= ( 1 << PF5 );
-}
-    
-// ----------------------------------------------------------------------------------------
-
-inline void led_green_1_off()
-{
-    PORTF |= ( 1 << PF4 );
-}
-
-inline void led_green_1_on()
-{
-    PORTF &= ~( 1 << PF4 );
-}
-    
-inline void led_green_1_toggle()
-{
-    PORTF ^= ( 1 << PF4 );
-}
-    
-// ----------------------------------------------------------------------------------------
-
 static const uint8_t odrctl_fatal_error_can_init = 1;
 static const uint8_t odrctl_fatal_error_can_comm = 2;
-static const uint8_t odrctl_fatal_error_motorctl_init = 3;
 static const uint8_t odrctl_fatal_error_exiting = 9;
 
 void odrctl_fatal_error( uint8_t fault )
@@ -268,14 +194,6 @@
 
     display_init();
 
-    //--    Initialize the Pololu Qik 2s12v10 motor controller.
-
-    motorctl_init();
-
-    //--    Initialize servo support hardware.
-
-    servo_init();
-
     //--    Initialize the CAN related functions and hardware.
 
     if ( ! canmsg_init() )
@@ -296,28 +214,46 @@
 
 ISR( TIMER0_COMP_vect )
 {
-    // update the internal timer
+    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.
+    //
+    //      led blink pattern: 60 milliseconds on, 150 off, 60 on
+    //      check emergency status: every 100 milliseconds
+    //      update the system status and display every 500 milliseconds
+    //      heartbeat: every 2.5 seconds
     
-    static uint8_t count = 0;
+    if ( count % 10 == 0 )
+    {
+        g_triggers |= trigger_check_emergency;
+    }
+
+    if ( count % 50 == 0 )
+    {
+        g_triggers |= ( trigger_update_status | trigger_update_display );
+    }
 
     switch ( ++count )
     {
-        case 50:
-        case 150:
-            g_triggers |= trigger_update_display;
-            g_triggers |= trigger_update_status;
+        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 100:
-        case 200:
-            g_triggers |= trigger_update_status;
-            g_triggers |= trigger_update_display;
-            g_triggers |= trigger_check_motorctl;
+        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 250:
-            g_triggers |= trigger_update_display;
-            g_triggers |= trigger_update_status;
             g_triggers |= trigger_send_heartbeat;
             count = 0;
     }
@@ -418,11 +354,31 @@
 
     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();
         
+        //--    Check for an emergency situation?
+
+        if ( g_triggers & trigger_check_emergency )
+        {
+            g_triggers &= ~trigger_check_emergency;
+
+            if ( status_is_emergency() )
+            {
+                led_red_on();
+            }
+            else
+            {
+                led_red_off();
+            }
+        }
+
         //--    Pending RF message?
 
         if ( rfcomm_rx_message_pending() )
@@ -436,17 +392,6 @@
             }
         }
 
-        //--    Suspend the motor control if the ESTOP mechanism is active.
-
-        if ( status_is_estop_active() )
-        {
-            motorctl_suspend();
-        }
-        else
-        {
-            motorctl_unsuspend();
-        }
-
         //--    Button states changes?
 
         if ( g_triggers & trigger_button_event )
@@ -470,24 +415,11 @@
             display_refresh();
         }
 
-        //--    Reinitialize the motor controller?
-
-        if ( g_triggers & trigger_check_motorctl )
-        {
-            g_triggers &= ~trigger_check_motorctl;
-
-            if ( motorctl_is_in_error() )
-            {
-                motorctl_init();
-            }
-        }
-
         //--    Time to send the status update message?
 
         if ( g_triggers & trigger_update_status )
         {
             g_triggers &= ~trigger_update_status;
-
             status_update();
         }
 
@@ -497,15 +429,7 @@
         {
             g_triggers &= ~trigger_send_heartbeat;
 
-            led_green_1_on();
-            spinwait_delay_ms( 60 );
-            led_green_1_off();
-            spinwait_delay_ms( 150 );
-            led_green_1_on();
-            spinwait_delay_ms( 60 );
-            led_green_1_off();
-
-            if ( ! canmsg_send_update() )
+            if ( ! canmsg_send_heartbeat() )
             {
                 ++can_comm_errors;
             }
--- a/main/robots/odr-controller/motorctl.cpp	Wed Aug 21 21:01:39 2013 -0700
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,253 +0,0 @@
-// ----------------------------------------------------------------------------------------
-//
-//  Copyright (C) 2012 Bob Cook
-//    
-//  Bob Cook Development, Robotics Library
-//  http://www.bobcookdev.com/rl/
-// 
-//  Pololu Qik2s12v10 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 <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()
-{
-    PORTE |= ( 1 << PE2 );
-}
-
-inline void motorctl_qik_reset_low()
-{
-    PORTE &= ~( 1 << PE2 );
-}
-
-// ----------------------------------------------------------------------------------------
-
-bool motorctl_init()
-{
-    //--    Qik motor controller reset on PORTE.2.
-    
-    DDRE |= ( 1 << PE2 );
-
-    //--    Qik motor controller error on PORTE.4. Turn off the pull-up.
-
-    DDRE &= ~( 1 << PE4 );
-    PORTE &= ~( 1 << PE4 );
-
-    //--    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 ( ( PINE & ( 1 << PE4 ) ) > 0 )
-    {
-        return true;
-    }
-
-    if ( s_motor_error ) return true;
-    return ( PINE & ( 1 << PE4 ) ) > 0;
-}
-
-// ----------------------------------------------------------------------------------------
-
-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 );
-        }
-    }
-}
-
-// ----------------------------------------------------------------------------------------
-
-void motorctl_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 );
-    }
-}
-
-// ----------------------------------------------------------------------------------------
-
-void motorctl_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 );
-    }
-}
-
-// ----------------------------------------------------------------------------------------
-
-int8_t motorctl_speed_front()
-{
-    return s_motor_speed_front;
-}
-
-int8_t motorctl_speed_rear()
-{
-    return s_motor_speed_rear;
-}
-
-// ----------------------------------------------------------------------------------------
-
--- a/main/robots/odr-controller/project_defs.h	Wed Aug 21 21:01:39 2013 -0700
+++ b/main/robots/odr-controller/project_defs.h	Wed Aug 21 21:02:10 2013 -0700
@@ -33,6 +33,7 @@
 // ----------------------------------------------------------------------------------------
 //  packages/avr/common/util
 
+#define PRJ_PRINTDEC_ENABLE_OUTPUT_TO_BUFFER
 #define PRJ_PRINTHEX_ENABLE_OUTPUT_TO_BUFFER
 
 // ----------------------------------------------------------------------------------------
@@ -110,6 +111,8 @@
 
 #define PRJ_RFCOMM_RX_BUFFER_SIZE  4
 
+#define PRJ_UTIL_CRC16_ENABLE
+
 // ----------------------------------------------------------------------------------------
 //  packages/common/pololu/qik2s12v10
 
--- a/main/robots/odr-controller/servos.cpp	Wed Aug 21 21:01:39 2013 -0700
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,174 +0,0 @@
-// ----------------------------------------------------------------------------------------
-//
-//  Copyright (C) 2011 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
-        | ( 0 << COM1C1 ) | ( 0 << COM1C0 )     // OC1C unused
-        | ( 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
-        | ( 0 << FOC1C )    // no force output compare
-        ;
-
-    ICR1 = 20000; // 10ms counting up, 10ms counting down (for 16Mhz clock)
-
-    DDRB |= ( 1 << PB5 ) | ( 1 << PB6 ); // front servo is on PB5, rear servo is on PB6
-   
-    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;
-}
-
-// ----------------------------------------------------------------------------------------
-
--- a/main/robots/odr-controller/status.cpp	Wed Aug 21 21:01:39 2013 -0700
+++ b/main/robots/odr-controller/status.cpp	Wed Aug 21 21:02:10 2013 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2011 Bob Cook
+//  Copyright (C) 2011-2013 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
@@ -35,15 +35,34 @@
 
 #include "packages/common/can/can_nodes.h"
 
+#include "leds.h"
+
 // ----------------------------------------------------------------------------------------
 
 static volatile uint8_t g_beats_without_mgr;
 static volatile uint8_t g_beats_without_estop;
+static volatile uint8_t g_beats_without_motion_ctl;
 static volatile uint8_t g_beats_without_sonarfront;
 
 static volatile uint8_t g_status_flags;
 
-static const uint8_t status_flag_sonar_front_enabled = ( 1 << 0 );
+static const uint8_t status_flag_emergency_active    = ( 1 << 0 );
+static const uint8_t status_flag_estop_triggered     = ( 1 << 1 );
+static const uint8_t status_flag_sonar_front_enabled = ( 1 << 2 );
+
+// ----------------------------------------------------------------------------------------
+
+static bool did_estop_timeout()
+{
+    return ( g_beats_without_estop > 2 ); // more than one second == ESTOP!
+}
+
+// ----------------------------------------------------------------------------------------
+
+static bool is_estop_override_enabled()
+{
+    return ( PINA & ( 1 << PA2 ) );
+}
 
 // ----------------------------------------------------------------------------------------
 
@@ -54,25 +73,22 @@
     g_beats_without_mgr = 100;
     g_beats_without_estop = 100;
     g_beats_without_sonarfront = 100;
+    g_beats_without_motion_ctl = 100;
     // g_beats_without_gps = 100;
 }
 
 // ----------------------------------------------------------------------------------------
 
-void status_update()
-{
-    if ( g_beats_without_mgr < 200 ) { ++g_beats_without_mgr; }
-    if ( g_beats_without_estop < 200 ) { ++g_beats_without_estop; }
-    if ( g_beats_without_sonarfront < 200 ) { ++g_beats_without_sonarfront; }
-}
-
-// ----------------------------------------------------------------------------------------
-
 void status_got_heartbeat( uint8_t source )
 {
     switch ( source )
     {
         case can_node_odr_manager:
+            g_beats_without_mgr = 0;
+            break;
+
+        case can_node_odr_motion:
+            g_beats_without_motion_ctl = 0;
             break;
 
         case can_node_odr_sonar_front:
@@ -86,6 +102,114 @@
 
 // ----------------------------------------------------------------------------------------
 
+void status_set_emergency()
+{
+    g_status_flags |= status_flag_emergency_active;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_got_emergency( uint8_t source )
+{
+    g_status_flags |= status_flag_emergency_active;
+
+    switch ( source )
+    {
+        case can_node_odr_manager:
+            break;
+
+        case can_node_odr_motion:
+            break;
+
+        case can_node_odr_sonar_front:
+            break;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_got_all_clear( uint8_t source )
+{
+    switch ( source )
+    {
+        case can_node_odr_manager:
+            g_status_flags &= ~status_flag_emergency_active;
+            break;
+
+        case can_node_odr_motion:
+            break;
+
+        case can_node_odr_sonar_front:
+            break;
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_update()
+{
+    //--    Increment all of the beat counters.
+
+    if ( g_beats_without_mgr < 200 ) { ++g_beats_without_mgr; }
+    if ( g_beats_without_estop < 200 ) { ++g_beats_without_estop; }
+    if ( g_beats_without_sonarfront < 200 ) { ++g_beats_without_sonarfront; }
+    if ( g_beats_without_motion_ctl < 200 ) { ++g_beats_without_motion_ctl; }
+
+    //--    Has the ESTOP condition transitioned? Note we ignore this check if the
+    //      override switch has been set.
+
+    if ( is_estop_override_enabled() )
+    {
+        // ESTOP not triggered due to override
+        g_status_flags &= ~status_flag_estop_triggered;
+
+        led_estop_override_on();
+
+        if ( status_is_emergency() )
+        {
+            canmsg_send_all_clear();
+        }
+    }
+    else if ( g_status_flags & status_flag_estop_triggered )
+    {
+        //--    Override off, the ESTOP is already triggered.
+
+        led_estop_override_off();
+
+        if ( ! did_estop_timeout() )
+        {
+            // switching from ESTOP triggered to not triggered
+            g_status_flags &= ~status_flag_estop_triggered;
+            canmsg_send_all_clear();
+        }
+        else
+        {
+            // keep sending the emergency until the ESTOP is not triggered
+            canmsg_send_emergency();
+        }
+    }
+    else
+    {
+        //--    Override off, the ESTOP is not currently triggered.
+
+        led_estop_override_off();
+
+        if ( did_estop_timeout() )
+        {
+            // switching from ESTOP not triggered to triggered
+            g_status_flags |= status_flag_estop_triggered;
+            canmsg_send_emergency();
+        }
+        else if ( status_is_emergency() )
+        {
+            // keep sending the all-clear until we are out of the emergency state
+            canmsg_send_all_clear();
+        }
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
 void status_got_mgr_update()
 {
     g_beats_without_mgr = 0;
@@ -116,6 +240,24 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool status_is_emergency()
+{
+    // more than two seconds == down
+    if ( g_beats_without_mgr > 5 )
+    {
+        return true;
+    }
+
+    if ( status_is_estop_active() )
+    {
+        return true;
+    }
+
+    return ( g_status_flags & status_flag_emergency_active ) > 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
 bool status_is_mgr_up()
 {
     // more than five seconds == down
@@ -126,19 +268,15 @@
 
 bool status_is_estop_active()
 {
-    // check for the ESTOP override
-    if ( PINA & ( 1 << PA2 ) )
-    {
-        PORTA |= ( 1 << PA3 );
-        return false;  // ignore the ESTOP status, dangerous!
-    }
-    else
-    {
-        PORTA &= ~( 1 << PA3 );
-    }
+    return ( g_status_flags & status_flag_estop_triggered ) > 0;
+}
+
+// ----------------------------------------------------------------------------------------
 
-    // more than one second == ESTOP!
-    return ( g_beats_without_estop > 1 );
+bool status_is_motion_ctl_alive()
+{
+    // more than four seconds == down
+    return ( g_beats_without_motion_ctl < 8 );
 }
 
 // ----------------------------------------------------------------------------------------