changeset 142:cf9723976178 main

Show the correct ESTOP status; increase the status check frequency to 2 Hz.
author Bob Cook <bob@bobcookdev.com>
date Mon, 21 May 2012 14:50:16 -0700
parents 880379afa4b3
children 403240057abc
files main/robots/odr-controller/func.h main/robots/odr-controller/main.cpp main/robots/odr-controller/status.cpp
diffstat 3 files changed, 43 insertions(+), 22 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-controller/func.h	Mon May 21 14:48:41 2012 -0700
+++ b/main/robots/odr-controller/func.h	Mon May 21 14:50:16 2012 -0700
@@ -35,13 +35,13 @@
 // ----------------------------------------------------------------------------------------
 
 void status_init();
-void status_do_heartbeat();
+void status_update();
 
 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();
+void status_got_rf_message( const uint8_t* data, uint8_t length );
 
 bool status_is_mgr_up();
 bool status_is_estop_active();
--- a/main/robots/odr-controller/main.cpp	Mon May 21 14:48:41 2012 -0700
+++ b/main/robots/odr-controller/main.cpp	Mon May 21 14:50:16 2012 -0700
@@ -1,6 +1,6 @@
 // ----------------------------------------------------------------------------------------
 //
-//  Copyright (C) 2011 Bob Cook
+//  Copyright (C) 2012 Bob Cook
 //    
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
@@ -41,6 +41,7 @@
 #include "packages/avr/rfcomm/rfcomm_rx.h"
 
 #include "packages/common/pololu/qik2s12v10.h"
+#include "packages/common/util/misc.h"
 
 // ----------------------------------------------------------------------------------------
 
@@ -51,6 +52,8 @@
 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 volatile uint8_t g_triggers;
 
@@ -231,7 +234,7 @@
 
     TIMSK |= ( 1 << OCIE0 );
 
-    //--    Set up a timer for RF reception at ~9.6 kHz on Timer2. This is 8 * 1.2 kHz,
+    //--    Set up a timer for RF reception at 2.50 kHz on Timer2. This is 8 * 312.5 Hz,
     //      the frequency of the transmitter.
 
     TCCR2 = 0
@@ -239,10 +242,10 @@
         | ( 0 << FOC2  )                                    // no force output compare
         | ( 1 << WGM21 ) | ( 0 << WGM20 )                   // CTC mode
         | ( 0 << COM21 ) | ( 0 << COM20 )                   // OC2 not used
-        | ( 0 << CS22  ) | ( 1 << CS21  ) | ( 1 << CS20 )   // clk/64
+        | ( 1 << CS22  ) | ( 0 << CS21  ) | ( 0 << CS20 )   // clk/256
         ; 
 
-    OCR2 = 26;
+    OCR2 = 25;
 
     TIMSK |= ( 1 << OCIE2 );
 
@@ -277,7 +280,8 @@
         odrctl_fatal_error( odrctl_fatal_error_can_init );
     }
 
-    //--    Initialize the RF receiver.
+    //--    Initialize the RF receiver. PD4 is the "power down" pin on the receiver,
+    //      set it low.
 
     rfcomm_rx_init();
 
@@ -296,15 +300,22 @@
     switch ( ++count )
     {
         case 50:
+        case 150:
+            g_triggers |= trigger_update_display;
+            g_triggers |= trigger_update_status;
+            break;
+
         case 100:
-        case 150:
         case 200:
+            g_triggers |= trigger_update_status;
             g_triggers |= trigger_update_display;
+            g_triggers |= trigger_check_motorctl;
             break;
 
         case 250:
             g_triggers |= trigger_update_display;
             g_triggers |= trigger_update_status;
+            g_triggers |= trigger_send_heartbeat;
             count = 0;
     }
 
@@ -373,8 +384,6 @@
 ISR( TIMER2_COMP_vect )
 {
     rfcomm_rx_run_task();
-
-//    TCNT2 = 0; // reset the timer to generate the same interrupt frequency later
 }
 
 // ----------------------------------------------------------------------------------------
@@ -415,8 +424,8 @@
 
         if ( rfcomm_rx_message_pending() )
         {
-            uint8_t incoming[ 20 ];
-            uint8_t length = 20;
+            uint8_t incoming[ 6 ];
+            uint8_t length = array_sizeof( incoming );
 
             if ( rfcomm_rx_read( incoming, &length ) )
             {
@@ -449,9 +458,14 @@
 
         //--    Reinitialize the motor controller?
 
-        if ( motorctl_is_in_error() )
+        if ( g_triggers & trigger_check_motorctl )
         {
-            motorctl_init();
+            g_triggers &= ~trigger_check_motorctl;
+
+            if ( motorctl_is_in_error() )
+            {
+                motorctl_init();
+            }
         }
 
         //--    Time to send the status update message?
@@ -460,6 +474,15 @@
         {
             g_triggers &= ~trigger_update_status;
 
+            status_update();
+        }
+
+        //--    Time to send the heartbeat message?
+
+        if ( g_triggers & trigger_send_heartbeat )
+        {
+            g_triggers &= ~trigger_send_heartbeat;
+
             led_green_1_on();
             spinwait_delay_ms( 60 );
             led_green_1_off();
@@ -468,8 +491,6 @@
             spinwait_delay_ms( 60 );
             led_green_1_off();
 
-            status_do_heartbeat();
-
             if ( ! canmsg_send_update() )
             {
                 ++can_comm_errors;
--- a/main/robots/odr-controller/status.cpp	Mon May 21 14:48:41 2012 -0700
+++ b/main/robots/odr-controller/status.cpp	Mon May 21 14:50:16 2012 -0700
@@ -25,7 +25,7 @@
 //  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 1 Hz (once per second).
+//  There is an implicit assumption our "heartbeat" is 2 Hz (twice per second).
 //
 // ----------------------------------------------------------------------------------------
 
@@ -57,7 +57,7 @@
 
 // ----------------------------------------------------------------------------------------
 
-void status_do_heartbeat()
+void status_update()
 {
     if ( g_beats_without_mgr < 200 ) { ++g_beats_without_mgr; }
     if ( g_beats_without_estop < 200 ) { ++g_beats_without_estop; }
@@ -117,15 +117,15 @@
 bool status_is_mgr_up()
 {
     // more than five seconds == down
-    return ( g_beats_without_mgr < 6 );
+    return ( g_beats_without_mgr < 10 );
 }
 
 // ----------------------------------------------------------------------------------------
 
 bool status_is_estop_active()
 {
-    // more than two seconds == ESTOP!
-    return ( g_beats_without_estop > 2 );
+    // more than one second == ESTOP!
+    return ( g_beats_without_estop > 1 );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -133,7 +133,7 @@
 bool status_is_sonar_front_up()
 {
     // more than four seconds == down
-    return ( g_beats_without_sonarfront < 5 );
+    return ( g_beats_without_sonarfront < 8 );
 }
 
 // ----------------------------------------------------------------------------------------