changeset 144:1f998d3a559a main

Extend the front sonar module to support four sensors (increased from three). Also increases the poll frequency of the sensors from 3 Hz to 16 Hz.
author Bob Cook <bob@bobcookdev.com>
date Sat, 07 Jul 2012 14:52:52 -0700
parents 403240057abc
children fa0eaf92e263 3ff923cfab5f
files main/packages/common/can/can_messages.h 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/motorctl.cpp main/robots/odr-controller/project_defs.h main/robots/odr-controller/status.cpp main/robots/odr-sonar-front/canmsgs.cpp main/robots/odr-sonar-front/func.h main/robots/odr-sonar-front/main.cpp main/robots/odr-sonar-front/project_defs.h main/robots/odr-sonar-front/sonar.cpp
diffstat 13 files changed, 287 insertions(+), 114 deletions(-) [+]
line wrap: on
line diff
--- a/main/packages/common/can/can_messages.h	Mon May 21 14:51:07 2012 -0700
+++ b/main/packages/common/can/can_messages.h	Sat Jul 07 14:52:52 2012 -0700
@@ -144,7 +144,8 @@
 typedef struct
 {
     uint16_t left;
-    uint16_t center;
+    uint16_t center_left;
+    uint16_t center_right;
     uint16_t right;
 
 } can_data_sonar_front;
--- a/main/robots/odr-controller/canmsgs.cpp	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-controller/canmsgs.cpp	Sat Jul 07 14:52:52 2012 -0700
@@ -130,7 +130,10 @@
 
 static void update_sonar_front( const can_data_sonar_front* data )
 {
-    display_update_sonar_front( data->left, data->center, data->right );
+    display_update_sonar_front( data->left,
+                                data->center_left,
+                                data->center_right,
+                                data->right );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr-controller/display.cpp	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-controller/display.cpp	Sat Jul 07 14:52:52 2012 -0700
@@ -53,7 +53,7 @@
 //      5-|servo f:+000  r:+000 |-
 //      6-|                     |-
 //      7-|     front sonar     |-
-//      8-|  0000  0000  0000   |-
+//      8-| 0000 0000 0000 0000 |-
 //      9-|                     |-
 //     10-|                     |-
 //      1-|                     |-
@@ -136,13 +136,20 @@
         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, sfe569_color_blue );
+    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, sfe569_color_blue );
+    sfe569_write( buf, 16, 4, sfe569_color_white, background );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -231,13 +238,13 @@
 {
     if ( ! status_is_sonar_front_up() )
     {
-        sfe569_write_pgm( PSTR("    offline     "),
-                          2, 8, sfe569_color_white, sfe569_color_red );
+        sfe569_write_pgm( PSTR("      offline      "),
+                          1, 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 );
+        sfe569_write_pgm( PSTR("      disabled     "),
+                          1, 8, sfe569_color_white, sfe569_color_blue );
     }
 }
 
@@ -307,28 +314,36 @@
 
 // ----------------------------------------------------------------------------------------
 
-void display_update_sonar_front( uint16_t left, uint16_t center, uint16_t right )
+void display_update_sonar_front
+(
+    uint16_t left,
+    uint16_t center_left,
+    uint16_t center_right,
+    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 );
+    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 = '\0';
-    sfe569_write( buf, 2, 8, sfe569_color_white, sfe569_color_blue );
+    sfe569_write( buf, 1, 8, sfe569_color_white, sfe569_color_blue );
 
-    p = print_hex( buf, center );
+    p = print_hex( buf, center_left );
     *p = '\0';
-    sfe569_write( buf, 8, 8, sfe569_color_white, sfe569_color_blue );
+    sfe569_write( buf, 6, 8, sfe569_color_white, sfe569_color_blue );
+
+    p = print_hex( buf, center_right );
+    *p = '\0';
+    sfe569_write( buf, 11, 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 );
+    sfe569_write( buf, 16, 8, sfe569_color_white, sfe569_color_blue );
 }
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr-controller/func.h	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-controller/func.h	Sat Jul 07 14:52:52 2012 -0700
@@ -56,7 +56,7 @@
 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_sonar_front( uint16_t l, uint16_t c_l, uint16_t c_r, uint16_t r );
 
 void display_update_button_1( bool down );
 void display_update_button_2( bool down );
@@ -86,6 +86,10 @@
 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 );
 
--- a/main/robots/odr-controller/main.cpp	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-controller/main.cpp	Sat Jul 07 14:52:52 2012 -0700
@@ -202,6 +202,9 @@
 
     DDRA |= ( 1 << PA3 );
 
+    DDRA  &= ~( 1 << PA2 );
+    PORTA |=  ( 1 << PA2 );
+
     //--    Blink the LEDs just to say hello.
     
     for ( uint8_t i = 0; i < 5; i++ )
@@ -433,6 +436,17 @@
             }
         }
 
+        //--    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 )
--- a/main/robots/odr-controller/motorctl.cpp	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-controller/motorctl.cpp	Sat Jul 07 14:52:52 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/
@@ -45,6 +45,7 @@
 // ----------------------------------------------------------------------------------------
 
 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;
@@ -105,8 +106,6 @@
 
 bool motorctl_check_status()
 {
-    if ( motorctl_is_in_error() ) return false;
-
     uint8_t qik_firmware_version;
     if ( ! pololu_qik2s12v10_firmware_version( &qik_firmware_version ) ) 
     {
@@ -114,6 +113,7 @@
         return false;
     }
 
+    s_motor_error = false;
     return true;
 }
 
@@ -121,12 +121,68 @@
 
 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 )
@@ -136,6 +192,11 @@
 
     s_motor_speed_front = speed;
 
+    if ( s_motor_suspended )
+    {
+        return;
+    }
+
     // yes, the motor leads are backwards thus forward == reverse
     
     if ( speed < 0 )
@@ -159,6 +220,11 @@
 
     s_motor_speed_rear = speed;
 
+    if ( s_motor_suspended )
+    {
+        return;
+    }
+
     // yes, the motor leads are backwards thus forward == reverse
     
     if ( speed < 0 )
--- a/main/robots/odr-controller/project_defs.h	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-controller/project_defs.h	Sat Jul 07 14:52:52 2012 -0700
@@ -63,7 +63,7 @@
 #define PRJ_MCP2515_SELECT_PORT     PORTB
 #define PRJ_MCP2515_SELECT_PIN      PB0
 
-#define PRJ_MCP2515_CANBUS_125_KHZ
+#define PRJ_MCP2515_CANBUS_250_KHZ
 
 #define PRJ_MCP2515_FREQUENCY_16_MHZ
 
--- a/main/robots/odr-controller/status.cpp	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-controller/status.cpp	Sat Jul 07 14:52:52 2012 -0700
@@ -31,6 +31,8 @@
 
 #include "func.h"
 
+#include <avr/io.h>
+
 #include "packages/common/can/can_nodes.h"
 
 // ----------------------------------------------------------------------------------------
@@ -124,6 +126,17 @@
 
 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 );
+    }
+
     // more than one second == ESTOP!
     return ( g_beats_without_estop > 1 );
 }
--- a/main/robots/odr-sonar-front/canmsgs.cpp	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-sonar-front/canmsgs.cpp	Sat Jul 07 14:52:52 2012 -0700
@@ -70,7 +70,13 @@
 
 // ----------------------------------------------------------------------------------------
 
-bool canmsg_send_sonar_front( uint16_t left, uint16_t center, uint16_t right )
+bool canmsg_send_sonar_front
+(
+    uint16_t left,
+    uint16_t center_left,
+    uint16_t center_right,
+    uint16_t right
+)
 {
     uint32_t msgid = can_build_message_id( can_node_odr_sonar_front,
                                            can_node_broadcast,
@@ -78,9 +84,10 @@
 
     can_data_sonar_front data;
 
-    data.left   = left;
-    data.center = center;
-    data.right  = right;
+    data.left         = left;
+    data.center_left  = center_left;
+    data.center_right = center_right;
+    data.right        = right;
 
     return m1can_send( msgid, reinterpret_cast< uint8_t* >( &data ), sizeof( data ) );
 }
--- a/main/robots/odr-sonar-front/func.h	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-sonar-front/func.h	Sat Jul 07 14:52:52 2012 -0700
@@ -44,7 +44,7 @@
 
 bool canmsg_init();
 bool canmsg_send_status( bool enabled );
-bool canmsg_send_sonar_front( uint16_t left, uint16_t center, uint16_t right );
+bool canmsg_send_sonar_front( uint16_t l, uint16_t cl, uint16_t rl, uint16_t r );
 bool canmsg_process_pending();
 
 // ----------------------------------------------------------------------------------------
--- a/main/robots/odr-sonar-front/main.cpp	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-sonar-front/main.cpp	Sat Jul 07 14:52:52 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/
@@ -65,11 +65,13 @@
     PORTD |= ( 1 << PD7 );
 }
 
+static inline void led_red_on() __attribute__((always_inline));
 static inline void led_red_on()
 {
     PORTD &= ~( 1 << PD7 );
 }
 
+static inline void led_red_toggle() __attribute__((always_inline));
 static inline void led_red_toggle()
 {
     PORTD ^= ( 1 << PD7 );
@@ -77,51 +79,60 @@
 
 // ----------------------------------------------------------------------------------------
 
-inline void led_yellow_off()
+static inline void led_yellow_off() __attribute__((always_inline));
+static inline void led_yellow_off()
 {
     PORTB |= ( 1 << PB2 );
 }
 
-inline void led_yellow_on()
+static inline void led_yellow_on() __attribute__((always_inline));
+static inline void led_yellow_on()
 {
     PORTB &= ~( 1 << PB2 );
 }
     
-inline void led_yellow_toggle()
+static inline void led_yellow_toggle() __attribute__((always_inline));
+static inline void led_yellow_toggle()
 {
     PORTB ^= ( 1 << PB2 );
 }
     
 // ----------------------------------------------------------------------------------------
 
-inline void led_green_2_off()
+static inline void led_green_2_off() __attribute__((always_inline));
+static inline void led_green_2_off()
 {
     PORTC |= ( 1 << PC4 );
 }
 
-inline void led_green_2_on()
+static inline void led_green_2_on() __attribute__((always_inline));
+static inline void led_green_2_on()
 {
     PORTC &= ~( 1 << PC4 );
 }
     
-inline void led_green_2_toggle()
+static inline void led_green_2_toggle() __attribute__((always_inline));
+static inline void led_green_2_toggle()
 {
     PORTC ^= ( 1 << PC4 );
 }
     
 // ----------------------------------------------------------------------------------------
 
-inline void led_green_1_off()
+static inline void led_green_1_off() __attribute__((always_inline));
+static inline void led_green_1_off()
 {
     PORTC |= ( 1 << PC5 );
 }
 
-inline void led_green_1_on()
+static inline void led_green_1_on() __attribute__((always_inline));
+static inline void led_green_1_on()
 {
     PORTC &= ~( 1 << PC5 );
 }
-    
-inline void led_green_1_toggle()
+
+static inline void led_green_1_toggle() __attribute__((always_inline));
+static inline void led_green_1_toggle()
 {
     PORTC ^= ( 1 << PC5 );
 }
@@ -233,6 +244,7 @@
     //--    Initialize the sonar subsystem.
 
     sonar_init();
+    sonar_start();
 
     //--    When we sleep we want the "idle" mode e.g. wake up on any interrupt.
 
@@ -250,12 +262,14 @@
     static uint8_t count = 0;
 
     // sonar status: every 2.5 seconds
-    // sonar update: three times every 2.5 seconds
+    // sonar update: every 0.5 seconds
     
     switch ( ++count )
     {
-        case 83:
-        case 167:
+        case 50:
+        case 100:
+        case 150:
+        case 200:
             g_triggers |= trigger_send_sonar_update;
             break;
 
@@ -286,6 +300,18 @@
 
         sleep_mode();
 
+        //--    Time to send a sonar update message?
+
+        if ( g_triggers & trigger_send_sonar_update )
+        {
+            g_triggers &= ~trigger_send_sonar_update;
+
+            if ( ! sonar_send_update() )
+            {
+                ++can_comm_errors;
+            }
+        }
+
         //--    Time to send the status message?
 
         if ( g_triggers & trigger_send_sonar_status )
@@ -306,18 +332,6 @@
             }
         }
 
-        //--    Time to send a sonar update message?
-
-        if ( g_triggers & trigger_send_sonar_update )
-        {
-            g_triggers &= ~trigger_send_sonar_update;
-
-            if ( ! sonar_send_update() )
-            {
-                ++can_comm_errors;
-            }
-        }
-
         //--    Any pending CAN messages to receive/process?
 
         if ( ! canmsg_process_pending() )
--- a/main/robots/odr-sonar-front/project_defs.h	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-sonar-front/project_defs.h	Sat Jul 07 14:52:52 2012 -0700
@@ -34,7 +34,7 @@
 //  packages/avr/can
 
 #define PRJ_M1CAN_ENABLE
-#define PRJ_M1CAN_CANBUS_125_KHZ
+#define PRJ_M1CAN_CANBUS_250_KHZ
 #define PRJ_M1CAN_TX_BUFFER_SIZE  8
 
 
--- a/main/robots/odr-sonar-front/sonar.cpp	Mon May 21 14:51:07 2012 -0700
+++ b/main/robots/odr-sonar-front/sonar.cpp	Sat Jul 07 14:52:52 2012 -0700
@@ -41,11 +41,12 @@
 
 // ----------------------------------------------------------------------------------------
 
-volatile uint8_t   g_curr_sensor;   // 0 = left, 1 = center, 2 = right, 3 = center
+volatile uint8_t   g_curr_sensor; // 0 = left, 1 = center left, 2 = center right, 3 = right
 
-filtbuf_u16< 4 >   g_sensor_left;
-filtbuf_u16< 4 >   g_sensor_center;
-filtbuf_u16< 4 >   g_sensor_right;
+volatile uint16_t  g_sensor_left;               // pin PD6
+volatile uint16_t  g_sensor_center_left;        // pin PD5
+volatile uint16_t  g_sensor_center_right;       // pin PB6
+volatile uint16_t  g_sensor_right;              // pin PB7
 
 // ----------------------------------------------------------------------------------------
 
@@ -64,7 +65,7 @@
 
         | ( 0 << ICNC1 ) | ( 0 << ICES1 )                   // not using input capture
         | ( 0 << WGM13 ) | ( 0 << WGM12 )                   // Normal mode
-        | ( 0 << CS12  ) | ( 1 << CS11  ) | ( 1 << CS10 )   // clk/64
+        | ( 0 << CS12  ) | ( 1 << CS11  ) | ( 0 << CS10 )   // clk/8
         ;
 
     TCNT1H = 0x00; TCNT1L = 0x00;
@@ -81,23 +82,18 @@
     //--    We need to advance to the next sensor then trigger an output pulse. After
     //      the trigger we switch the pin to an input for sensing the hold-off period
     //      and the reception of the final result.
+    //
+    //      To reduce the frequency, we skip 1, 3, 5 and 7.
     
     g_curr_sensor++;
-    g_curr_sensor %= 4;
+    g_curr_sensor %= 8;
 
     switch ( g_curr_sensor )
     {
         case 0:
-            // switch PORTB.7 to an output, set it high
-            DDRB  |= ( 1 << PB7 );
-            PORTB |= ( 1 << PB7 );
-            break;
-
-        case 1:
-        case 3:
-            // switch PORTB.6 to an output, set it high
-            DDRB  |= ( 1 << PB6 );
-            PORTB |= ( 1 << PB6 );
+            // switch PORTD.6 to an output, set it high
+            DDRD  |= ( 1 << PD6 );
+            PORTD |= ( 1 << PD6 );
             break;
 
         case 2:
@@ -105,6 +101,18 @@
             DDRD  |= ( 1 << PD5 );
             PORTD |= ( 1 << PD5 );
             break;
+
+        case 4:
+            // switch PORTB.6 to an output, set it high
+            DDRB  |= ( 1 << PB6 );
+            PORTB |= ( 1 << PB6 );
+            break;
+
+        case 6:
+            // switch PORTB.7 to an output, set it high
+            DDRB  |= ( 1 << PB7 );
+            PORTB |= ( 1 << PB7 );
+            break;
     }
 
     spinwait_delay_us( 5 );
@@ -112,15 +120,22 @@
     switch ( g_curr_sensor )
     {
         case 0:
-            // set PORTB.7 low then switch it to an input
-            PORTB &= ~( 1 << PB7 );
-            DDRB  &= ~( 1 << PB7 );
-            // enable pin-change interrupt
-            PCMSK0 |= ( 1 << PCINT7 );
+            // set PORTD.6 low then switch it to an input
+            PORTD &= ~( 1 << PD6 );
+            DDRD  &= ~( 1 << PD6 );
+            // enable pin-change interrupt 22
+            PCMSK2 |= ( 1 << PCINT22 );
             break;
 
-        case 1:
-        case 3:
+        case 2:
+            // set PORTD.5 low then switch it to an input
+            PORTD &= ~( 1 << PD5 );
+            DDRD  &= ~( 1 << PD5 );
+            // enable pin-change interrupt 21
+            PCMSK2 |= ( 1 << PCINT21 );
+            break;
+
+        case 4:
             // set PORTB.6 low then switch it to an input
             PORTB &= ~( 1 << PB6 );
             DDRB  &= ~( 1 << PB6 );
@@ -128,12 +143,12 @@
             PCMSK0 |= ( 1 << PCINT6 );
             break;
 
-        case 2:
-            // set PORTD.5 low then switch it to an input
-            PORTD &= ~( 1 << PD5 );
-            DDRD  &= ~( 1 << PD5 );
+        case 6:
+            // set PORTB.7 low then switch it to an input
+            PORTB &= ~( 1 << PB7 );
+            DDRB  &= ~( 1 << PB7 );
             // enable pin-change interrupt
-            PCMSK2 |= ( 1 << PCINT21 );
+            PCMSK0 |= ( 1 << PCINT7 );
             break;
     }
 }
@@ -142,9 +157,26 @@
 
 ISR( PCINT0_vect )
 {
-    //--    Pin-change interrupt for PCINT0, could be PORTB.6 or PORTB.7.
+    //--    Pin-change interrupt, could be PORTB.6 (center right) or PORTB.7 (right).
     
-    if ( g_curr_sensor == 0 ) // PORTB.7
+    if ( g_curr_sensor == 4 ) // PORTB.6
+    {
+        if ( ( PINB & ( 1 << PB6 ) ) != 0 )
+        {
+            // rising edge => hold-off period over, reset timer
+            TCNT1H = 0x00; TCNT1L = 0x00;
+        }
+        else
+        {
+            // falling edge => end of sample
+            uint16_t sample = TCNT1L;
+            sample |= static_cast< uint16_t >( TCNT1H ) << 8;
+            g_sensor_center_right = sample;
+            // disable pin-change interrupt
+            PCMSK0 &= ~( 1 << PCINT6 );
+        }
+    }
+    else // PORTB.7
     {
         if ( ( PINB & ( 1 << PB7 ) ) != 0 )
         {
@@ -156,14 +188,22 @@
             // falling edge => end of sample
             uint16_t sample = TCNT1L;
             sample |= static_cast< uint16_t >( TCNT1H ) << 8;
-            g_sensor_left.append( sample );
+            g_sensor_right = sample;
             // disable pin-change interrupt
             PCMSK0 &= ~( 1 << PCINT7 );
         }
     }
-    else // PORTB.6
+}
+
+// ----------------------------------------------------------------------------------------
+
+ISR( PCINT2_vect )
+{
+    //--    Pin-change interrupt, could be PORTD.5 (center left) or PORTD.6 (left).
+    
+    if ( g_curr_sensor == 0 ) // PORTD.6
     {
-        if ( ( PINB & ( 1 << PB6 ) ) != 0 )
+        if ( ( PIND & ( 1 << PD6 ) ) != 0 )
         {
             // rising edge => hold-off period over, reset timer
             TCNT1H = 0x00; TCNT1L = 0x00;
@@ -173,32 +213,27 @@
             // falling edge => end of sample
             uint16_t sample = TCNT1L;
             sample |= static_cast< uint16_t >( TCNT1H ) << 8;
-            g_sensor_center.append( sample );
+            g_sensor_left = sample;
             // disable pin-change interrupt
-            PCMSK0 &= ~( 1 << PCINT6 );
+            PCMSK2 &= ~( 1 << PCINT22 );
         }
     }
-}
-
-// ----------------------------------------------------------------------------------------
-
-ISR( PCINT2_vect )
-{
-    //--    Pin-change interrupt for PCINT0, we are only expecting PORTD.5 here.
-    
-    if ( ( PIND & ( 1 << PD5 ) ) != 0 )
+    else // PORTD.5
     {
-        // rising edge => hold-off period over, reset timer
-        TCNT1H = 0x00; TCNT1L = 0x00;
-    }
-    else
-    {
-        // falling edge => end of sample
-        uint16_t sample = TCNT1L;
-        sample |= static_cast< uint16_t >( TCNT1H ) << 8;
-        g_sensor_right.append( sample );
-        // disable pin-change interrupt
-        PCMSK2 &= ~( 1 << PCINT21 );
+        if ( ( PIND & ( 1 << PD5 ) ) != 0 )
+        {
+            // rising edge => hold-off period over, reset timer
+            TCNT1H = 0x00; TCNT1L = 0x00;
+        }
+        else
+        {
+            // falling edge => end of sample
+            uint16_t sample = TCNT1L;
+            sample |= static_cast< uint16_t >( TCNT1H ) << 8;
+            g_sensor_center_left = sample;
+            // disable pin-change interrupt
+            PCMSK2 &= ~( 1 << PCINT21 );
+        }
     }
 }
 
@@ -238,9 +273,10 @@
         return true; // no error
     }
 
-    return canmsg_send_sonar_front( g_sensor_left.average(),
-                                    g_sensor_center.average(),
-                                    g_sensor_right.average() );
+    return canmsg_send_sonar_front( g_sensor_left,
+                                    g_sensor_center_left,
+                                    g_sensor_center_right,
+                                    g_sensor_right );
 }
 
 // ----------------------------------------------------------------------------------------