changeset 132:0ea0246d4a70 main

Latest version of the Outdoor Robot controller.
author Bob Cook <bob@bobcookdev.com>
date Tue, 27 Dec 2011 12:38:18 -0800
parents 03f847019642
children 98172ca268ae
files main/robots/odr-controller/canmsgs.cpp main/robots/odr-controller/display.cpp main/robots/odr-controller/func.h main/robots/odr-controller/main.cpp main/robots/odr-controller/project_defs.h main/robots/odr-controller/status.cpp
diffstat 6 files changed, 230 insertions(+), 155 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-controller/canmsgs.cpp	Mon Dec 26 15:50:37 2011 -0800
+++ b/main/robots/odr-controller/canmsgs.cpp	Tue Dec 27 12:38:18 2011 -0800
@@ -41,6 +41,8 @@
 #include "packages/common/can/can_nodes.h"
 #include "packages/common/pololu/qik2s12v10.h"
 
+#include "packages/avr/lcd/sfe569/sfe569.h"
+
 // ----------------------------------------------------------------------------------------
 
 bool canmsg_init()
@@ -75,7 +77,7 @@
                                            can_node_broadcast,
                                            can_dataid_odrctl_update );
 
-    can_data_odrctl_update info;
+    can_data_odrctl_update info = { 0 };
 
     info.estop    = status_is_estop_active();
     info.motorctl = motorctl_check_status();
@@ -126,6 +128,13 @@
 
 // ----------------------------------------------------------------------------------------
 
+static void update_sonar_front( const can_data_sonar_front* data )
+{
+    display_update_sonar_front( data->left, data->center, data->right );
+}
+
+// ----------------------------------------------------------------------------------------
+
 static void got_mgr_update( const can_data_odrmgr_update* data, uint8_t length )
 {
     status_got_mgr_update();
@@ -178,6 +187,18 @@
         case can_dataid_motor_speed:
             update_motors( reinterpret_cast< can_data_motor_speed* >( &recvdata ) );
             break;
+
+        case can_dataid_sonar_front_state_enabled:
+            status_got_sonar_front_enabled();
+            break;
+
+        case can_dataid_sonar_front_state_disabled:
+            status_got_sonar_front_disabled();
+            break;
+
+        case can_dataid_sonar_front:
+            update_sonar_front( reinterpret_cast< can_data_sonar_front* >( &recvdata ) );
+            break;
     }
 
     return true; // no errors
--- a/main/robots/odr-controller/display.cpp	Mon Dec 26 15:50:37 2011 -0800
+++ b/main/robots/odr-controller/display.cpp	Tue Dec 27 12:38:18 2011 -0800
@@ -27,12 +27,12 @@
 //
 // ----------------------------------------------------------------------------------------
 
-//#include <avr/interrupt.h>
-//#include <avr/io.h>
 #include <avr/pgmspace.h>
 
 #include "func.h"
 
+#include "packages/common/util/printutils.h"
+
 #include "packages/avr/lcd/sfe569/sfe569.h"
 
 // always after other includes
@@ -52,8 +52,8 @@
 //      4-|motor f:+000  r:+000 |-
 //      5-|servo f:+000  r:+000 |-
 //      6-|                     |-
-//      7-|                     |-
-//      8-|                     |-
+//      7-|     front sonar     |-
+//      8-|  0000  0000  0000   |-
 //      9-|                     |-
 //     10-|                     |-
 //      1-|                     |-
@@ -74,39 +74,9 @@
 static volatile uint8_t g_spinner_count;
 
 // ----------------------------------------------------------------------------------------
-#if 0
-static void str_hex( char* str, uint8_t byte )
+
+static void str_small_int( char* str, int8_t value )
 {
-    uint8_t high_nybble = byte >> 4;
-
-    if ( high_nybble < 0x0a )
-    {
-        *str++ = ( '0' + high_nybble );
-    }
-    else
-    {
-        *str++ = ( 'A' + ( high_nybble - 0x0a ) );
-    }
-
-    byte &= 0x0f;
-
-    if ( byte < 0x0a )
-    {
-        *str++ = ( '0' + byte );
-    }
-    else
-    {
-        *str++ = ( 'A' + ( byte - 0x0a ) );
-    }
-}
-#endif
-
-// ----------------------------------------------------------------------------------------
-
-static void str_small_int( char* str, uint8_t value )
-{
-    // this only works for numbers less than 200
-
     if ( value >= 100 )
     {
         *str++ = '1';
@@ -128,96 +98,66 @@
     }
 
     *str++ = ( '0' + value );
+
+    *str = '\0';
 }
 
 // ----------------------------------------------------------------------------------------
 
-static void update_motor_info()
+static void str_small_int_with_sign( char* str, int8_t value )
 {
-    char buf[ 24 ];
-
-    if ( ! motorctl_check_status() )
-    {
-        //                      012345678901234567890
-        sfe569_write_pgm( PSTR("motor error          "), 0, 4,
-                          sfe569_color_white, sfe569_color_red );
-        return;
-    }
-
-    //                    012345678901234567890
-    //                    motor f:sxxx  r:sxxx  
-    strncpy_P( buf, PSTR("motor f:      r:     "), sizeof( buf )  - 1 );
-
-    int8_t value = motorctl_speed_front();
-
     if ( value < 0 )
     {
         value = -value;
-        buf[ 8 ] = '-';
-    }
-    else if ( value > 0 )
-    {
-        buf[ 8 ] = '+';
-    }
-
-    str_small_int( &buf[ 9 ], value );
-
-    value = motorctl_speed_rear();
-
-    if ( value < 0 )
-    {
-        value = -value;
-        buf[ 16 ] = '-';
+        *str++ = '-';
     }
     else if ( value > 0 )
     {
-        buf[ 16 ] = '+';
+        *str++ = '+';
+    }
+    else
+    {
+        *str++ = ' ';
     }
 
-    str_small_int( &buf[ 17 ], value );
-
-    sfe569_write( buf, 0, 4, sfe569_color_white, sfe569_color_blue );
+    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;
+    }
+
+    char buf[ 6 ];
+
+    str_small_int_with_sign( buf, motorctl_speed_front() );
+    sfe569_write( buf, 8, 4, sfe569_color_white, sfe569_color_blue );
+
+    str_small_int_with_sign( buf, motorctl_speed_rear() );
+    sfe569_write( buf, 16, 4, sfe569_color_white, sfe569_color_blue );
+}
+
+// ----------------------------------------------------------------------------------------
+//  012345678901234567890
+//  servo f:sxxx  r:sxxx  
 
 static void update_servo_info()
 {
-    char buf[ 24 ];
-
-    //                    012345678901234567890
-    //                    servo f:sxxx  r:sxxx  
-    strncpy_P( buf, PSTR("servo f:      r:     "), sizeof( buf ) );
-
-    int8_t value = servo_position_front();
-
-    if ( value < 0 )
-    {
-        value = -value;
-        buf[ 8 ] = '-';
-    }
-    else if ( value > 0 )
-    {
-        buf[ 8 ] = '+';
-    }
+    char buf[ 6 ];
 
-    str_small_int( &buf[ 9 ], value );
-
-    value = servo_position_rear();
+    str_small_int_with_sign( buf, servo_position_front() );
+    sfe569_write( buf, 8, 5, sfe569_color_white, sfe569_color_blue );
 
-    if ( value < 0 )
-    {
-        value = -value;
-        buf[ 16 ] = '-';
-    }
-    else if ( value > 0 )
-    {
-        buf[ 16 ] = '+';
-    }
-
-    str_small_int( &buf[ 17 ], value );
-
-    sfe569_write( buf, 0, 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 );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -227,6 +167,9 @@
     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 );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -284,12 +227,29 @@
 
 // ----------------------------------------------------------------------------------------
 
+static void update_sonar_front_info()
+{
+    if ( ! status_is_sonar_front_up() )
+    {
+        sfe569_write_pgm( PSTR("    offline     "),
+                          2, 8, sfe569_color_white, sfe569_color_red );
+    }
+    else if ( ! status_is_sonar_front_enabled() )
+    {
+        sfe569_write_pgm( PSTR("    disabled    "),
+                          2, 8, sfe569_color_white, sfe569_color_blue );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
 void display_refresh()
 {
     update_heartbeat_flag();
     update_estop_flag();
     update_motor_info();
     update_servo_info();
+    update_sonar_front_info();
 
     ++g_spinner_count;
     g_spinner_count %= 4;
@@ -347,3 +307,29 @@
 
 // ----------------------------------------------------------------------------------------
 
+void display_update_sonar_front( uint16_t left, uint16_t center, uint16_t right )
+{
+    char  buf[ 6 ];
+    char* p = buf;
+
+    *p++ = ' ';
+    *p++ = ' ';
+    *p++ = '\0';
+    sfe569_write( buf,  6, 8, sfe569_color_white, sfe569_color_blue );
+    sfe569_write( buf, 12, 8, sfe569_color_white, sfe569_color_blue );
+
+    p = print_hex( buf, left );
+    *p = '\0';
+    sfe569_write( buf, 2, 8, sfe569_color_white, sfe569_color_blue );
+
+    p = print_hex( buf, center );
+    *p = '\0';
+    sfe569_write( buf, 8, 8, sfe569_color_white, sfe569_color_blue );
+
+    p = print_hex( buf, right );
+    *p = '\0';
+    sfe569_write( buf, 14, 8, sfe569_color_white, sfe569_color_blue );
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-controller/func.h	Mon Dec 26 15:50:37 2011 -0800
+++ b/main/robots/odr-controller/func.h	Tue Dec 27 12:38:18 2011 -0800
@@ -40,9 +40,13 @@
 void status_got_heartbeat( uint8_t node );
 void status_got_mgr_update();
 void status_got_rf_message( const uint8_t* data, uint8_t length );
+void status_got_sonar_front_enabled();
+void status_got_sonar_front_disabled();
 
 bool status_is_mgr_up();
 bool status_is_estop_active();
+bool status_is_sonar_front_up();
+bool status_is_sonar_front_enabled();
 
 // ----------------------------------------------------------------------------------------
 
@@ -52,6 +56,8 @@
 void display_set_mgr_status( const char* msg, uint8_t length );
 void display_clear_mgr_status();
 
+void display_update_sonar_front( uint16_t left, uint16_t center, uint16_t right );
+
 void display_update_button_1( bool down );
 void display_update_button_2( bool down );
 
--- a/main/robots/odr-controller/main.cpp	Mon Dec 26 15:50:37 2011 -0800
+++ b/main/robots/odr-controller/main.cpp	Tue Dec 27 12:38:18 2011 -0800
@@ -29,26 +29,22 @@
 
 #include <avr/interrupt.h>
 #include <avr/io.h>
-#include <avr/pgmspace.h>
-
-#include <ctype.h>
-#include <stdio.h>
-#include <string.h>
+#include <avr/sleep.h>
 
 #include "project_defs.h"
 #include "func.h"
 
 #include "packages/avr/device/int_helpers.h"
+#include "packages/avr/device/simavr_helpers.h"
 #include "packages/avr/device/spinwait.h"
 
-#include "packages/avr/lcd/sfe569/sfe569.h"
-
 #include "packages/avr/rfcomm/rfcomm_rx.h"
 
-// always after other includes
-#include "packages/avr/device/workaround34734.h"
+#include "packages/common/pololu/qik2s12v10.h"
 
-#include "packages/common/pololu/qik2s12v10.h"
+// ----------------------------------------------------------------------------------------
+
+SIMAVR_DECLARE_DEVICE_PARAMETERS();
 
 // ----------------------------------------------------------------------------------------
 
@@ -199,6 +195,10 @@
     DDRF |= ( 1 << PF4 ) | ( 1 << PF5 ) | ( 1 << PF6 ) | ( 1 << PF7 );
     leds_off();
 
+    //--    Motor status LED on PORTA.3
+
+    DDRA |= ( 1 << PA3 );
+
     //--    Blink the LEDs just to say hello.
     
     for ( uint8_t i = 0; i < 5; i++ )
@@ -246,10 +246,18 @@
 
     TIMSK |= ( 1 << OCIE2 );
 
+    //--    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();
 
+    //--    Initialize the status package.
+
+    status_init();
+
     //--    Initialize the display package (the LCD display).
 
     display_init();
@@ -285,16 +293,19 @@
     
     static uint8_t count = 0;
 
-    if ( count == 50 )
+    switch ( ++count )
     {
-        g_triggers |= trigger_update_display;
-    }
+        case 50:
+        case 100:
+        case 150:
+        case 200:
+            g_triggers |= trigger_update_display;
+            break;
 
-    if ( ++count > 100 )
-    {
-        g_triggers |= trigger_update_status;
-        g_triggers |= trigger_update_display;
-        count = 0;
+        case 250:
+            g_triggers |= trigger_update_display;
+            g_triggers |= trigger_update_status;
+            count = 0;
     }
 
     // check the state of the buttons
@@ -354,7 +365,7 @@
 
     // reset the timer to generate the same interrupt frequency later
 
-    TCNT0 = 0;
+//    TCNT0 = 0;
 }
 
 // ----------------------------------------------------------------------------------------
@@ -363,7 +374,7 @@
 {
     rfcomm_rx_run_task();
 
-    TCNT2 = 0; // reset the timer to generate the same interrupt frequency later
+//    TCNT2 = 0; // reset the timer to generate the same interrupt frequency later
 }
 
 // ----------------------------------------------------------------------------------------
@@ -395,28 +406,11 @@
 
     for ( ;; )
     {
-        //--    Check for warning and/or error conditions.
-
-        bool warning = false;
-
-        if ( can_comm_errors > 50 )
-        {
-            odrctl_fatal_error( odrctl_fatal_error_can_comm );
-        }
-        else if ( can_comm_errors > 10 )
-        {
-            warning = true;
-        }
-
-        if ( warning )
-        {
-            led_yellow_on();
-        }
-        else
-        {
-            led_yellow_off();
-        }
-
+        //--    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();
+        
         //--    Pending RF message?
 
         if ( rfcomm_rx_message_pending() )
@@ -427,9 +421,6 @@
             if ( rfcomm_rx_read( incoming, &length ) )
             {
                 status_got_rf_message( incoming, length );
-
-                incoming[length] = '\0';
-                sfe569_write( (char*)incoming, 0, 8, sfe569_color_white, sfe569_color_blue );
             }
         }
 
@@ -499,7 +490,27 @@
             }
         }
 
-        //--    End of main loop.
+        //--    Check for warning and/or error conditions.
+
+        bool warning = false;
+
+        if ( can_comm_errors > 50 )
+        {
+            odrctl_fatal_error( odrctl_fatal_error_can_comm );
+        }
+        else if ( can_comm_errors > 10 )
+        {
+            warning = true;
+        }
+
+        if ( warning )
+        {
+            led_yellow_on();
+        }
+        else
+        {
+            led_yellow_off();
+        }
     }
 
     odrctl_fatal_error( odrctl_fatal_error_exiting );
--- a/main/robots/odr-controller/project_defs.h	Mon Dec 26 15:50:37 2011 -0800
+++ b/main/robots/odr-controller/project_defs.h	Tue Dec 27 12:38:18 2011 -0800
@@ -31,6 +31,11 @@
 #define PRJ_CPU_FREQ    16000000L
 
 // ----------------------------------------------------------------------------------------
+//  packages/avr/common/util
+
+#define PRJ_PRINTHEX_ENABLE_OUTPUT_TO_BUFFER
+
+// ----------------------------------------------------------------------------------------
 //  packages/avr/device/spi
 
 #define PRJ_SPI_ENABLE_MASTER
@@ -49,6 +54,7 @@
 
 #define PRJ_MCP2515_ENABLE
 
+#define PRJ_MCP2515_USE_HW_RESET
 #define PRJ_MCP2515_RESET_DDR      DDRE
 #define PRJ_MCP2515_RESET_PORT     PORTE
 #define PRJ_MCP2515_RESET_PIN      PE7
@@ -63,7 +69,7 @@
 
 #define PRJ_MCP2515_USE_INTERRUPT_RXTX
 #define PRJ_MCP2515_TX_BUFFER_SIZE      4
-#define PRJ_MCP2515_RX_BUFFER_SIZE      4
+#define PRJ_MCP2515_RX_BUFFER_SIZE      8
 
 // ----------------------------------------------------------------------------------------
 //  packages/avr/lcd/sfe569
@@ -102,6 +108,8 @@
 #define PRJ_RFCOMM_RX_INPUT PIND
 #define PRJ_RFCOMM_RX_PIN   PD2
 
+#define PRJ_RFCOMM_RX_BUFFER_SIZE  4
+
 // ----------------------------------------------------------------------------------------
 //  packages/common/pololu/qik2s12v10
 
--- a/main/robots/odr-controller/status.cpp	Mon Dec 26 15:50:37 2011 -0800
+++ b/main/robots/odr-controller/status.cpp	Tue Dec 27 12:38:18 2011 -0800
@@ -37,6 +37,11 @@
 
 static volatile uint8_t g_beats_without_mgr;
 static volatile uint8_t g_beats_without_estop;
+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 );
 
 // ----------------------------------------------------------------------------------------
 
@@ -46,6 +51,7 @@
     
     g_beats_without_mgr = 100;
     g_beats_without_estop = 100;
+    g_beats_without_sonarfront = 100;
     // g_beats_without_gps = 100;
 }
 
@@ -55,6 +61,7 @@
 {
     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; }
 }
 
 // ----------------------------------------------------------------------------------------
@@ -66,6 +73,9 @@
         case can_node_odr_manager:
             break;
 
+        case can_node_odr_sonar_front:
+            break;
+
 //        case can_node_gps:
 //            g_beats_without_gps = 0;
 //            break;
@@ -88,17 +98,50 @@
 
 // ----------------------------------------------------------------------------------------
 
+void status_got_sonar_front_enabled()
+{
+    g_beats_without_sonarfront = 0;
+    g_status_flags |= status_flag_sonar_front_enabled;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void status_got_sonar_front_disabled()
+{
+    g_beats_without_sonarfront = 0;
+    g_status_flags &= ~status_flag_sonar_front_enabled;
+}
+
+// ----------------------------------------------------------------------------------------
+
 bool status_is_mgr_up()
 {
-    return ( g_beats_without_mgr < 3 ); // more than two seconds == down
+    // more than five seconds == down
+    return ( g_beats_without_mgr < 6 );
 }
 
 // ----------------------------------------------------------------------------------------
 
 bool status_is_estop_active()
 {
-    return ( g_beats_without_estop > 2 ); // more than two seconds == ESTOP!
+    // more than two seconds == ESTOP!
+    return ( g_beats_without_estop > 2 );
 }
 
 // ----------------------------------------------------------------------------------------
 
+bool status_is_sonar_front_up()
+{
+    // more than four seconds == down
+    return ( g_beats_without_sonarfront < 5 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+bool status_is_sonar_front_enabled()
+{
+    return ( g_status_flags & status_flag_sonar_front_enabled ) > 0;
+}
+
+// ----------------------------------------------------------------------------------------
+