changeset 181:a84d43219fbc main

Initial checkin for the more-or-less complete motion control functionality for wheel speed.
author Bob Cook <bob@bobcookdev.com>
date Sat, 13 Apr 2013 20:29:38 -0700
parents b72d6a919c6c
children b716d7cb9c78
files main/robots/odr-motion/canmsgs.cpp main/robots/odr-motion/encoders.cpp main/robots/odr-motion/func.h main/robots/odr-motion/jamfile main/robots/odr-motion/leds.inl main/robots/odr-motion/main.cpp main/robots/odr-motion/status.cpp main/robots/odr-motion/wheels.cpp
diffstat 8 files changed, 443 insertions(+), 48 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-motion/canmsgs.cpp	Sat Apr 13 20:28:29 2013 -0700
+++ b/main/robots/odr-motion/canmsgs.cpp	Sat Apr 13 20:29:38 2013 -0700
@@ -66,8 +66,23 @@
 
 // ----------------------------------------------------------------------------------------
 
-void send_motor_speed_msg( int8_t front, int8_t rear )
+bool canmsg_send_wheel_status()
 {
+    can_data_wheel_status data;
+
+    data.actual_rpm_front = encoder_wheel_rpm_front();
+    data.actual_rpm_rear  = wheel_speed_front(); //encoder_wheel_rpm_rear();
+
+    data.actual_motor_front = motorctl_speed_front();
+    data.actual_motor_rear  = motorctl_speed_rear();
+
+    uint32_t msgid = can_build_message_id( can_node_odr_motion,
+                                           can_node_broadcast,
+                                           can_dataid_wheel_status );
+
+    return m1can_send( msgid,
+                       reinterpret_cast< uint8_t* >( &data ),
+                       sizeof( data ) );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -79,9 +94,9 @@
 
 // ----------------------------------------------------------------------------------------
 
-static void update_motors( const can_data_motor_speed* data )
+static void update_speed( const can_data_wheel_speed* data )
 {
-    // motorctl_set_speed( data->motor_front, data->motor_rear );
+    wheel_speed_set( data->rpm_front, data->rpm_rear );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -105,7 +120,7 @@
 
     if ( dstnode != can_node_broadcast && dstnode != can_node_odr_motion )
     {
-        return true; // not for us, everything ok
+//        return true; // not for us, everything ok
     }
 
     switch ( dataid )
@@ -126,8 +141,8 @@
             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_speed:
+            update_speed( reinterpret_cast< can_data_wheel_speed* >( &recvdata ) );
             break;
     }
 
--- a/main/robots/odr-motion/encoders.cpp	Sat Apr 13 20:28:29 2013 -0700
+++ b/main/robots/odr-motion/encoders.cpp	Sat Apr 13 20:29:38 2013 -0700
@@ -5,7 +5,7 @@
 //  Bob Cook Development, Robotics Library
 //  http://www.bobcookdev.com/rl/
 // 
-//  Wheel encoder functions for the Outdoor Robot controller node.
+//  Motor encoder functions for the Outdoor Robot controller node.
 //
 //  Permission is hereby granted, free of charge, to any person obtaining a copy
 //  of this software and associated documentation files (the "Software"), to deal
@@ -27,21 +27,11 @@
 //
 // ----------------------------------------------------------------------------------------
 
-//#include <avr/interrupt.h>
-//#include <avr/io.h>
-//#include <avr/sleep.h>
-
 #include "project_defs.h"
-
 #include "func.h"
 
 #include "packages/avr/sensors/ls7366/ls7366.h"
 
-//#include "packages/avr/device/int_helpers.h"
-//#include "packages/avr/device/spinwait.h"
-
-//#include "packages/common/util/misc.h"
-
 // ----------------------------------------------------------------------------------------
 //  The encoder from US Digital gives 120 ticks per revolution of the motor, prior to
 //  gear reduction. This robot uses E4P-120-079-HT.
@@ -56,8 +46,10 @@
 //  1 wheel RPM = 18,540 ticks per minute = 309 ticks per second
 //
 
-static const uint32_t k_ticks_per_wheel_revolution = 18540L;
-static const uint32_t k_ticks_per_wheel_revolution_per_second = 309L;
+static const int32_t k_ticks_per_wheel_revolution = 18540L;
+static const int32_t k_ticks_per_wheel_revolution_per_second = 309L;
+
+static const int32_t k_min_ticks_for_one_rpm = 10;
 
 // ----------------------------------------------------------------------------------------
 
@@ -75,34 +67,56 @@
 
 // ----------------------------------------------------------------------------------------
 
-static volatile uint8_t g_wheel_rpm_front;
-static volatile uint8_t g_wheel_rpm_rear;
+static volatile int8_t g_wheel_rpm_front;
+static volatile int8_t g_wheel_rpm_rear;
 
 // ----------------------------------------------------------------------------------------
 
 static void refresh_front()
 {
-    static uint32_t s_diffs_array[ 10 ];
-    static uint8_t  s_index = 0;
+    static int32_t s_diffs_array[ 10 ];
+    static uint8_t s_index = 0;
 
     static uint32_t s_previous_cntr = 0;
 
     uint32_t cntr = ls7366_read_counter_1();
 
     uint8_t dir;
-    s_diffs_array[ s_index++ ] = ls7366_compute_diff( s_previous_cntr, cntr, &dir );
+    int32_t diff = ls7366_compute_diff( s_previous_cntr, cntr, &dir );
+
+    if ( dir == 0 ) { diff *= -1; }
+
+    s_diffs_array[ s_index++ ] = diff;
     s_index %= 10;
 
-    uint32_t sum_of_diffs = 0;
+    int32_t sum_of_diffs = 0;
 
     for ( uint8_t i = 0; i < 10; ++i )
     {
         sum_of_diffs += s_diffs_array[ i ];
     }
 
-    g_wheel_rpm_front = static_cast< uint8_t >(
+    // this function is called 100 times per second, we add up the last ten values, so
+    // multiply the result by 10 to get RPM; to smooth the result, we scale the sum of
+    // values by 100
+
+    g_wheel_rpm_front = static_cast< int8_t >(
                       ( sum_of_diffs * 100 )
-                      / ( k_ticks_per_wheel_revolution_per_second * 100 ) );
+                      / ( k_ticks_per_wheel_revolution_per_second * 10 ) );
+
+    // special treatment to insure zero really means zero
+
+    if ( g_wheel_rpm_front == 0 )
+    {
+        if ( sum_of_diffs > k_min_ticks_for_one_rpm )
+        {
+            g_wheel_rpm_front = 1;
+        }
+        else if ( sum_of_diffs < -k_min_ticks_for_one_rpm )
+        {
+            g_wheel_rpm_front = -1;
+        }
+    }
 
     s_previous_cntr = cntr;
 }
@@ -119,19 +133,41 @@
     uint32_t cntr = ls7366_read_counter_2();
 
     uint8_t dir;
-    s_diffs_array[ s_index++ ] = ls7366_compute_diff( s_previous_cntr, cntr, &dir );
+    int32_t diff = ls7366_compute_diff( s_previous_cntr, cntr, &dir );
+
+    if ( dir == 0 ) { diff *= -1; }
+
+    s_diffs_array[ s_index++ ] = diff;
     s_index %= 10;
 
-    uint32_t sum_of_diffs = 0;
+    int32_t sum_of_diffs = 0;
 
     for ( uint8_t i = 0; i < 10; ++i )
     {
         sum_of_diffs += s_diffs_array[ i ];
     }
 
-    g_wheel_rpm_rear = static_cast< uint8_t >(
+    // this function is called 100 times per second, we add up the last ten values, so
+    // multiply the result by 10 to get RPM; to smooth the result, we scale the sum of
+    // values by 100
+
+    g_wheel_rpm_rear = static_cast< int8_t >(
                      ( sum_of_diffs * 100 )
-                     / ( k_ticks_per_wheel_revolution_per_second * 100 ) );
+                     / ( k_ticks_per_wheel_revolution_per_second * 10 ) );
+
+    // special treatment to insure zero really means zero
+
+    if ( g_wheel_rpm_rear == 0 )
+    {
+        if ( sum_of_diffs > k_min_ticks_for_one_rpm )
+        {
+            g_wheel_rpm_rear = 1;
+        }
+        else if ( sum_of_diffs < -k_min_ticks_for_one_rpm )
+        {
+            g_wheel_rpm_rear = -1;
+        }
+    }
 
     s_previous_cntr = cntr;
 }
@@ -146,14 +182,14 @@
 
 // ----------------------------------------------------------------------------------------
 
-uint8_t encoder_wheel_rpm_front()
+int8_t encoder_wheel_rpm_front()
 {
     return g_wheel_rpm_front;
 }
 
 // ----------------------------------------------------------------------------------------
 
-uint8_t encoder_wheel_rpm_rear()
+int8_t encoder_wheel_rpm_rear()
 {
     return g_wheel_rpm_rear;
 }
--- a/main/robots/odr-motion/func.h	Sat Apr 13 20:28:29 2013 -0700
+++ b/main/robots/odr-motion/func.h	Sat Apr 13 20:29:38 2013 -0700
@@ -48,8 +48,16 @@
 bool canmsg_init();
 bool canmsg_send_emergency();
 bool canmsg_send_heartbeat();
+bool canmsg_send_wheel_status();
 bool canmsg_process_pending();
-void send_motor_speed_msg( int8_t front, int8_t rear ); // TEMPORARY...
+
+// ----------------------------------------------------------------------------------------
+
+void wheel_speed_refresh();
+void wheel_speed_set( int8_t front, int8_t rear );
+
+int8_t wheel_speed_front();
+int8_t wheel_speed_rear();
 
 // ----------------------------------------------------------------------------------------
 
@@ -68,8 +76,8 @@
 
 void encoder_refresh();
 
-uint8_t encoder_wheel_rpm_front();
-uint8_t encoder_wheel_rpm_rear();
+int8_t encoder_wheel_rpm_front();
+int8_t encoder_wheel_rpm_rear();
 
 // ----------------------------------------------------------------------------------------
 
--- a/main/robots/odr-motion/jamfile	Sat Apr 13 20:28:29 2013 -0700
+++ b/main/robots/odr-motion/jamfile	Sat Apr 13 20:29:38 2013 -0700
@@ -39,6 +39,7 @@
       leds.cpp
       motorctl.cpp
       status.cpp servos.cpp
+      wheels.cpp
       packages.avr.can.pkg
       packages.avr.device.pkg
       packages.avr.sensors.ls7366.pkg
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/leds.inl	Sat Apr 13 20:29:38 2013 -0700
@@ -0,0 +1,111 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  LED functions for the Outdoor Robot motion control board.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+#include <avr/io.h>
+
+//inline void led_red_off() __attribute__((always_inline));
+inline void led_red_off()
+{
+    PORTD |= ( 1 << PD7 );
+}
+
+//inline void led_red_on() __attribute__((always_inline));
+inline void led_red_on()
+{
+    PORTD &= ~( 1 << PD7 );
+}
+
+//inline void led_red_toggle() __attribute__((always_inline));
+inline void led_red_toggle()
+{
+    PORTD ^= ( 1 << PD7 );
+}
+
+// ----------------------------------------------------------------------------------------
+
+inline void led_yellow_off() __attribute__((always_inline));
+inline void led_yellow_off()
+{
+    PORTB |= ( 1 << PB2 );
+}
+
+inline void led_yellow_on() __attribute__((always_inline));
+inline void led_yellow_on()
+{
+    PORTB &= ~( 1 << PB2 );
+}
+    
+inline void led_yellow_toggle() __attribute__((always_inline));
+inline void led_yellow_toggle()
+{
+    PORTB ^= ( 1 << PB2 );
+}
+    
+// ----------------------------------------------------------------------------------------
+
+inline void led_green_2_off() __attribute__((always_inline));
+inline void led_green_2_off()
+{
+    PORTC |= ( 1 << PC4 );
+}
+
+inline void led_green_2_on() __attribute__((always_inline));
+inline void led_green_2_on()
+{
+    PORTC &= ~( 1 << PC4 );
+}
+    
+inline void led_green_2_toggle() __attribute__((always_inline));
+inline void led_green_2_toggle()
+{
+    PORTC ^= ( 1 << PC4 );
+}
+    
+// ----------------------------------------------------------------------------------------
+
+inline void led_green_1_off() __attribute__((always_inline));
+inline void led_green_1_off()
+{
+    PORTC |= ( 1 << PC5 );
+}
+
+inline void led_green_1_on() __attribute__((always_inline));
+inline void led_green_1_on()
+{
+    PORTC &= ~( 1 << PC5 );
+}
+
+inline void led_green_1_toggle() __attribute__((always_inline));
+inline void led_green_1_toggle()
+{
+    PORTC ^= ( 1 << PC5 );
+}
+    
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/odr-motion/main.cpp	Sat Apr 13 20:28:29 2013 -0700
+++ b/main/robots/odr-motion/main.cpp	Sat Apr 13 20:29:38 2013 -0700
@@ -48,10 +48,12 @@
 
 // ----------------------------------------------------------------------------------------
 
-static const uint8_t trigger_send_heartbeat = ( 1 << 0 );
-static const uint8_t trigger_update_encoder = ( 1 << 1 );
-static const uint8_t trigger_check_motorctl = ( 1 << 2 );
-static const uint8_t trigger_is_emergency   = ( 1 << 3 );
+static const uint8_t trigger_main_loop_ok    = ( 1 << 0 );
+static const uint8_t trigger_send_heartbeat  = ( 1 << 1 );
+static const uint8_t trigger_update_encoder  = ( 1 << 2 );
+static const uint8_t trigger_adjust_wheels   = ( 1 << 3 );
+static const uint8_t trigger_check_motorctl  = ( 1 << 4 );
+static const uint8_t trigger_check_emergency = ( 1 << 5 );
 
 static volatile uint8_t g_triggers;
 
@@ -183,9 +185,13 @@
 {
     static uint8_t count = 0;
 
+    //--    The trigger_main_loop_ok flag indicates the main loop is "alive" e.g. still
+    //      running. We only blink the "heartbeat" LED if it continues to toggle.
+    //
     // encoder update: every 10 milliseconds
     // led blink pattern: 60 milliseconds on, 150 off, 60 on
     // check ESTOP status: every 100 milliseconds
+    // adjust the motor speeds every 500 milliseconds
     // check motor controller: every 1.25 seconds
     // heartbeat: every 2.5 seconds
     
@@ -193,25 +199,31 @@
 
     if ( count % 10 == 0 )
     {
-        g_triggers |= trigger_is_emergency;
+        g_triggers |= trigger_check_emergency;
+    }
+
+    if ( count % 50 == 0 )
+    {
+        g_triggers |= trigger_adjust_wheels;
     }
 
     switch ( ++count )
     {
         case 10:
-            led_green_1_on();
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_on(); }
             break;
 
         case 16:
-            led_green_1_off();
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_off(); }
             break;
 
         case 31:
-            led_green_1_on();
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_on(); }
             break;
 
         case 37:
-            led_green_1_off();
+            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_off(); }
+            g_triggers &= ~trigger_main_loop_ok;
             break;
 
         case 125:
@@ -380,6 +392,10 @@
 
     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).
 
@@ -392,38 +408,69 @@
             g_triggers &= ~trigger_update_encoder;
 
             encoder_refresh();
-            //adjust_motors();
+        }
+
+        //--    Adjust the wheel speeds?
+
+        if ( g_triggers & trigger_adjust_wheels )
+        {
+            g_triggers &= ~trigger_adjust_wheels;
+
+            wheel_speed_refresh();
+            canmsg_send_wheel_status();
+
+            if ( wheel_speed_front() != 0 )
+            {
+                led_green_2_on();
+            }
+            else
+            {
+                led_green_2_off();
+            }
         }
 
         //--    Check for an emergency situation?
 
-        if ( g_triggers & trigger_is_emergency )
+        if ( g_triggers & trigger_check_emergency )
         {
-            g_triggers &= ~trigger_is_emergency;
+            g_triggers &= ~trigger_check_emergency;
 
             if ( status_is_emergency() )
             {
+//                led_red_on();
                 motorctl_suspend();
                 // suspend the servos
             }
             else
             {
+//                led_red_off();
                 motorctl_unsuspend();
                 // unsuspend the servos
             }
         }
 
+#if 1
         //--    Check the motor controller status?
 
         if ( g_triggers & trigger_check_motorctl )
         {
             g_triggers &= ~trigger_check_motorctl;
 
+            if ( ! motorctl_check_status() )
+            {
+                led_red_on();
+            }
+            else
+            {
+                led_red_off();
+            }
+
             if ( motorctl_is_in_error() )
             {
                 motorctl_init();
             }
         }
+#endif
 
         //--    Time to send the heartbeat message?
 
--- a/main/robots/odr-motion/status.cpp	Sat Apr 13 20:28:29 2013 -0700
+++ b/main/robots/odr-motion/status.cpp	Sat Apr 13 20:29:38 2013 -0700
@@ -30,6 +30,7 @@
 // ----------------------------------------------------------------------------------------
 
 #include "func.h"
+#include "leds.h"
 
 #include <avr/io.h>
 
@@ -63,6 +64,8 @@
 
 void status_got_heartbeat( uint8_t source )
 {
+    led_red_toggle();
+
     switch ( source )
     {
         case can_node_odr_manager:
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr-motion/wheels.cpp	Sat Apr 13 20:29:38 2013 -0700
@@ -0,0 +1,174 @@
+// ----------------------------------------------------------------------------------------
+//
+//  Copyright (C) 2013 Bob Cook
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+// 
+//  Wheel speed and position functions for the Outdoor Robot motion control node.
+//
+//  Permission is hereby granted, free of charge, to any person obtaining a copy
+//  of this software and associated documentation files (the "Software"), to deal
+//  in the Software without restriction, including without limitation the rights
+//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+//  copies of the Software, and to permit persons to whom the Software is
+//  furnished to do so, subject to the following conditions:
+// 
+//  The above copyright notice and this permission notice shall be included in
+//  all copies or substantial portions of the Software.
+// 
+//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+//  THE SOFTWARE.
+//
+// ----------------------------------------------------------------------------------------
+
+//#include <ctype.h>
+//#include <string.h>
+
+#include "project_defs.h"
+#include "func.h"
+
+// ----------------------------------------------------------------------------------------
+
+static const int8_t k_max_motor_error_constant = 20;
+static const int8_t k_error_gain = 3;
+
+static volatile int8_t s_wheel_rpm_front;
+static volatile int8_t s_motor_front;
+
+static volatile int8_t s_wheel_rpm_rear;
+static volatile int8_t s_motor_rear;
+
+// ----------------------------------------------------------------------------------------
+
+void wheel_init()
+{
+    s_wheel_rpm_front = 0;
+    s_motor_front = 0;
+    s_wheel_rpm_rear = 0;
+    s_motor_rear = 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void adjust_motor_front( int8_t delta )
+{
+    int16_t m = s_motor_front;
+    m += delta;
+
+    if ( m < -127 )
+    {
+        s_motor_front = -127;
+    }
+    else if ( m > 127 )
+    {
+        s_motor_front = 127;
+    }
+    else
+    {
+        s_motor_front = static_cast< int8_t >( m );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+static void adjust_motor_rear( int8_t delta )
+{
+    int16_t m = s_motor_rear;
+    m += delta;
+
+    if ( m < -127 )
+    {
+        s_motor_rear = -127;
+    }
+    else if ( m > 127 )
+    {
+        s_motor_rear = 127;
+    }
+    else
+    {
+        s_motor_rear = static_cast< int8_t >( m );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void wheel_speed_refresh()
+{
+    int8_t error_front = s_wheel_rpm_front - encoder_wheel_rpm_front();
+
+    if ( error_front > 1 )
+    {
+        error_front *= k_error_gain;
+    }
+
+    if ( error_front > k_max_motor_error_constant )
+    {
+        adjust_motor_front( k_max_motor_error_constant );
+    }
+    else if ( error_front < -k_max_motor_error_constant )
+    {
+        adjust_motor_front( -k_max_motor_error_constant );
+    }
+    else
+    {
+        adjust_motor_front( error_front );
+    }
+
+    int8_t error_rear = s_wheel_rpm_rear - encoder_wheel_rpm_rear();
+
+    if ( error_rear > 1 )
+    {
+        error_rear *= k_error_gain;
+    }
+
+    if ( error_rear > k_max_motor_error_constant )
+    {
+        adjust_motor_rear( k_max_motor_error_constant );
+    }
+    else if ( error_rear < -k_max_motor_error_constant )
+    {
+        adjust_motor_rear( k_max_motor_error_constant );
+    }
+    else
+    {
+        adjust_motor_rear( error_rear );
+    }
+
+    if ( error_front != 0 || error_rear != 0 )
+    {
+        motorctl_set_speed( s_motor_front, s_motor_rear );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void wheel_speed_set( int8_t front, int8_t rear )
+{
+    s_wheel_rpm_front = front;
+    s_wheel_rpm_rear  = rear;
+
+    wheel_speed_refresh();
+}
+
+// ----------------------------------------------------------------------------------------
+
+int8_t wheel_speed_front()
+{
+    return s_wheel_rpm_front;
+}
+
+// ----------------------------------------------------------------------------------------
+
+int8_t wheel_speed_rear()
+{
+    return s_wheel_rpm_rear;
+}
+
+// ----------------------------------------------------------------------------------------
+