changeset 195:d6621e43b91b main

Code for the motion control module.
author Bob Cook <bob@bobcookdev.com>
date Wed, 21 Aug 2013 21:01:39 -0700
parents 0faaa371df10
children aa2665165208
files main/robots/odr-motion/canmsgs.cpp main/robots/odr-motion/encoders.cpp main/robots/odr-motion/func.h main/robots/odr-motion/main.cpp main/robots/odr-motion/servos.cpp main/robots/odr-motion/status.cpp main/robots/odr-motion/wheels.cpp
diffstat 7 files changed, 125 insertions(+), 200 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr-motion/canmsgs.cpp	Wed Aug 21 06:19:06 2013 -0700
+++ b/main/robots/odr-motion/canmsgs.cpp	Wed Aug 21 21:01:39 2013 -0700
@@ -57,6 +57,15 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool canmsg_send_all_clear()
+{
+    return m1can_send( can_build_message_id( can_node_odr_motion,
+                                             can_node_broadcast,
+                                             can_dataid_all_clear ), 0, 0 );
+}
+
+// ----------------------------------------------------------------------------------------
+
 bool canmsg_send_heartbeat()
 {
     return m1can_send( can_build_message_id( can_node_odr_motion,
@@ -71,11 +80,14 @@
     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_rpm_rear  = encoder_wheel_rpm_rear();
 
     data.actual_motor_front = motorctl_speed_front();
     data.actual_motor_rear  = motorctl_speed_rear();
 
+    data.angle_front = servo_position_front();
+    data.angle_rear  = servo_position_rear();
+
     uint32_t msgid = can_build_message_id( can_node_odr_motion,
                                            can_node_broadcast,
                                            can_dataid_wheel_status );
@@ -87,9 +99,9 @@
 
 // ----------------------------------------------------------------------------------------
 
-static void update_servos( const can_data_servo_position* data )
+static void update_position( const can_data_wheel_position* data )
 {
-    servo_set_positions( data->servo_front, data->servo_rear );
+    servo_set_positions( data->angle_front, data->angle_rear );
 }
 
 // ----------------------------------------------------------------------------------------
@@ -120,7 +132,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 )
@@ -134,11 +146,12 @@
             break;
 
         case can_dataid_heartbeat:
+        case can_dataid_odrmgr_update:
             status_got_heartbeat( srcnode );
             break;
 
-        case can_dataid_servo_position:
-            update_servos( reinterpret_cast< can_data_servo_position* >( &recvdata ) );
+        case can_dataid_wheel_position:
+            update_position( reinterpret_cast< can_data_wheel_position* >( &recvdata ) );
             break;
 
         case can_dataid_wheel_speed:
--- a/main/robots/odr-motion/encoders.cpp	Wed Aug 21 06:19:06 2013 -0700
+++ b/main/robots/odr-motion/encoders.cpp	Wed Aug 21 21:01:39 2013 -0700
@@ -84,6 +84,7 @@
     uint8_t dir;
     int32_t diff = ls7366_compute_diff( s_previous_cntr, cntr, &dir );
 
+    // note: direction of wheel rotation is different front vs. rear
     if ( dir == 0 ) { diff *= -1; }
 
     s_diffs_array[ s_index++ ] = diff;
@@ -135,7 +136,8 @@
     uint8_t dir;
     int32_t diff = ls7366_compute_diff( s_previous_cntr, cntr, &dir );
 
-    if ( dir == 0 ) { diff *= -1; }
+    // note: direction of wheel rotation is different front vs. rear
+    if ( dir != 0 ) { diff *= -1; }
 
     s_diffs_array[ s_index++ ] = diff;
     s_index %= 10;
--- a/main/robots/odr-motion/func.h	Wed Aug 21 06:19:06 2013 -0700
+++ b/main/robots/odr-motion/func.h	Wed Aug 21 21:01:39 2013 -0700
@@ -37,6 +37,8 @@
 void status_init();
 void status_update();
 
+void status_set_emergency();
+
 void status_got_emergency( uint8_t node );
 void status_got_all_clear( uint8_t node );
 void status_got_heartbeat( uint8_t node );
@@ -47,6 +49,7 @@
 
 bool canmsg_init();
 bool canmsg_send_emergency();
+bool canmsg_send_all_clear();
 bool canmsg_send_heartbeat();
 bool canmsg_send_wheel_status();
 bool canmsg_process_pending();
@@ -64,6 +67,9 @@
 void servo_init();
 void servo_shutdown();
 
+void servo_suspend();
+void servo_unsuspend();
+
 void servo_set_positions( int8_t front, int8_t rear );
 
 int8_t servo_position_front();
--- a/main/robots/odr-motion/main.cpp	Wed Aug 21 06:19:06 2013 -0700
+++ b/main/robots/odr-motion/main.cpp	Wed Aug 21 21:01:39 2013 -0700
@@ -54,6 +54,7 @@
 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 const uint8_t trigger_update_status   = ( 1 << 6 );
 
 static volatile uint8_t g_triggers;
 
@@ -154,6 +155,10 @@
 
     TIMSK0 |= ( 1 << OCIE0A );
 
+    //--    Initialize the status subsystem.
+
+    status_init();
+
     //--    Initialize the Pololu Qik 2s12v10 motor controller.
 
     motorctl_init();
@@ -163,6 +168,10 @@
     DDRD |= ( 1 << PD3 ); // this is the SS pin, required to be an output for SPI master
     ls7366_init();
 
+    //--    Initialize servo support hardware.
+
+    servo_init();
+
     //--    Initialize the CAN related functions and hardware.
 
     if ( ! canmsg_init() )
@@ -188,12 +197,13 @@
     //--    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
+    //      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
+    //      update the system status every 500 milliseconds
+    //      check motor controller: every 1.25 seconds
+    //      heartbeat: every 2.5 seconds
     
     g_triggers |= trigger_update_encoder;
 
@@ -204,7 +214,7 @@
 
     if ( count % 50 == 0 )
     {
-        g_triggers |= trigger_adjust_wheels;
+        g_triggers |= ( trigger_adjust_wheels | trigger_update_status );
     }
 
     switch ( ++count )
@@ -238,153 +248,13 @@
 }
 
 // ----------------------------------------------------------------------------------------
-//  The encoder from US Digital gives 120 ticks per revolution of the motor, prior to
-//  gear reduction. This robot uses E4P-120-079-HT.
-//
-//  The motor gives a reduction of 50:1 e.g. every 50 revolutions equals one revolution
-//  of the output shaft.
-//
-//  The differential gearbox between the motor output shaft and wheel contains 11 teeth
-//  on the shaft side, 34 teeth on the wheel side (reduction of 3.09:1).
-//  
-//  1 wheel revolution = 3.09 shaft revolutions = 154.5 motor revolutions = 18,540 ticks
-//  1 wheel RPM = 18,540 ticks per minute = 309 ticks per second
-//
-
-//const uint32_t k_ticks_per_wheel_revolution = 18540L;
-//const uint32_t k_ticks_per_wheel_revolution_per_second = 309L;
-
-// ----------------------------------------------------------------------------------------
-
-static volatile uint8_t g_wheel_rpm_setpt_front;
-static volatile uint8_t g_wheel_rpm_setpt_rear;
-
-#if 0
-void motorctl_set_speed( int8_t front, int8_t rear )
-{
-    if ( front < -23 )
-    {
-        g_dps_setpt_front = 230;
-    }
-    else if ( front < 0 )
-    {
-        g_dps_setpt_front = -front * 10;
-    }
-    else if ( front > 23 )
-    {
-        g_dps_setpt_front = 230;
-    }
-    else
-    {
-        g_dps_setpt_front = front * 10;
-    }
-}
-#endif
-
-// ----------------------------------------------------------------------------------------
-//  This function samples the motor encoders at 10 Hz, and stores the wheel rpm.
-//
-
-#if 0
-static volatile uint8_t g_wheel_rpm_front;
-static volatile uint8_t g_wheel_rpm_rear;
-
-void encoder_update_front()
-{
-    static uint32_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 );
-    s_index %= 10;
-
-    uint32_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 >(
-                      ( sum_of_diffs * 100 )
-                      / ( k_ticks_per_wheel_revolution_per_second * 100 ) );
-
-    s_previous_cntr = cntr;
-}
-
-void encoder_update_rear()
-{
-    static uint32_t s_diffs_array[ 10 ];
-    static uint8_t  s_index = 0;
-
-    static uint32_t s_previous_cntr = 0;
-
-    uint32_t cntr = ls7366_read_counter_2();
-
-    uint8_t dir;
-    s_diffs_array[ s_index++ ] = ls7366_compute_diff( s_previous_cntr, cntr, &dir );
-    s_index %= 10;
-
-    uint32_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 >(
-                     ( sum_of_diffs * 100 )
-                     / ( k_ticks_per_wheel_revolution_per_second * 100 ) );
-
-    s_previous_cntr = cntr;
-}
-#endif
-
-// ----------------------------------------------------------------------------------------
-
-static volatile int8_t g_motor_speed_front;
-static volatile int8_t g_motor_speed_rear;
-
-#if 0
-void adjust_motors()
-{
-    bool do_update = false;
-
-    if ( ( g_motor_dps + 1 ) < g_dps_setpt_front )
-    {
-        if ( g_motor_speed_front < 128 )
-        {
-            do_update = true;
-            ++g_motor_speed_front;
-        }
-    }
-    else if ( ( g_motor_dps - 1 ) > g_dps_setpt_front )
-    {
-        if ( g_motor_speed_front > 0 )
-        {
-            do_update = true;
-            --g_motor_speed_front;
-        }
-    }
-
-    if ( do_update )
-    {
-        send_motor_speed_msg( g_motor_speed_front, 0 );
-    }
-}
-#endif
-
-// ----------------------------------------------------------------------------------------
 
 int main()
 {
     //--    Initialize the hardware and send a "hello we started" message.
 
     odrmot_hw_init();
+    canmsg_send_all_clear();
 
     //--    Loop forever responding to CAN messages and system status.
 
@@ -410,25 +280,6 @@
             encoder_refresh();
         }
 
-        //--    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_check_emergency )
@@ -437,40 +288,56 @@
 
             if ( status_is_emergency() )
             {
-//                led_red_on();
+                led_red_on();
                 motorctl_suspend();
-                // suspend the servos
+                servo_suspend();
             }
             else
             {
-//                led_red_off();
+                led_red_off();
                 motorctl_unsuspend();
-                // unsuspend the servos
+                servo_unsuspend();
             }
         }
 
-#if 1
-        //--    Check the motor controller status?
+        //--    Time to adjust the wheel speeds?
+
+        if ( g_triggers & trigger_adjust_wheels )
+        {
+            g_triggers &= ~trigger_adjust_wheels;
+
+            if ( ! status_is_emergency() )
+            {
+                // unless its an emergency situation...
+                wheel_speed_refresh();
+            }
+
+            canmsg_send_wheel_status();
+        }
+
+        //--    Time to 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();
-            }
+            motorctl_check_status();
 
             if ( motorctl_is_in_error() )
             {
                 motorctl_init();
             }
+
+            if ( motorctl_is_in_error() )
+            {
+                status_set_emergency();
+                canmsg_send_emergency();
+            }
+            else if ( status_is_emergency() )
+            {
+                canmsg_send_all_clear();
+            }
         }
-#endif
 
         //--    Time to send the heartbeat message?
 
@@ -484,6 +351,15 @@
             }
         }
 
+        //--    Time to check status?
+
+        if ( g_triggers & trigger_update_status )
+        {
+            g_triggers &= ~trigger_update_status;
+
+            status_update();
+        }
+
         //--    Any pending CAN messages to receive/process?
 
         if ( ! canmsg_process_pending() )
--- a/main/robots/odr-motion/servos.cpp	Wed Aug 21 06:19:06 2013 -0700
+++ b/main/robots/odr-motion/servos.cpp	Wed Aug 21 21:01:39 2013 -0700
@@ -77,8 +77,8 @@
 
     ICR1 = 20000; // 10ms counting up, 10ms counting down (for 16Mhz clock)
 
-    DDRC |= ( 1 << PC1 );   // front servo is on PC1
-    DDRD |= ( 1 << PD2 );   // rear servo is on PD2
+    DDRC |= ( 1 << PC1 );   // front servo is on PC1 (OC1B)
+    DDRD |= ( 1 << PD2 );   // rear servo is on PD2 (OC1A)
    
     center_servos();    // initialize to center positions 
 }
@@ -88,6 +88,23 @@
 void servo_shutdown()
 {
     TCCR1A = 0; // Timer1 disabled
+    TCCR1B = 0;
+    TCCR1C = 0;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void servo_suspend()
+{
+    // turn off the clock
+    TCCR1B &= ~( ( 1 << CS12  ) | ( 1 << CS11 ) | ( 1 << CS10 ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
+void servo_unsuspend()
+{
+    TCCR1B |= ( 0 << CS12  ) | ( 1 << CS11 ) | ( 0 << CS10 ); // clk/8
 }
 
 // ----------------------------------------------------------------------------------------
@@ -109,10 +126,10 @@
 static void center_servos()
 {
     s_servo_position_front = 0;
-    OCR1A = pgm_read_word( g_front_servo_table + s_servo_position_front + 12 );
+    OCR1B = 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 );
+    OCR1A = pgm_read_word( g_rear_servo_table  + s_servo_position_rear  + 12 );
 }
 
 void servo_set_positions( int8_t front, int8_t rear )
@@ -132,8 +149,8 @@
 
     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 );
+        OCR1B = pgm_read_word( g_front_servo_table + s_servo_position_front + 12 );
+        OCR1A = pgm_read_word( g_rear_servo_table  + s_servo_position_rear  + 12 );
 
         if ( s_servo_position_front > f )
         {
--- a/main/robots/odr-motion/status.cpp	Wed Aug 21 06:19:06 2013 -0700
+++ b/main/robots/odr-motion/status.cpp	Wed Aug 21 21:01:39 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/
@@ -64,8 +64,6 @@
 
 void status_got_heartbeat( uint8_t source )
 {
-    led_red_toggle();
-
     switch ( source )
     {
         case can_node_odr_manager:
@@ -83,6 +81,13 @@
 
 // ----------------------------------------------------------------------------------------
 
+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;
@@ -103,7 +108,7 @@
 bool status_is_emergency()
 {
     // more than two seconds == down
-    if ( g_beats_without_mgr > 4 )
+    if ( g_beats_without_mgr > 5 )
     {
         return true;
     }
--- a/main/robots/odr-motion/wheels.cpp	Wed Aug 21 06:19:06 2013 -0700
+++ b/main/robots/odr-motion/wheels.cpp	Wed Aug 21 21:01:39 2013 -0700
@@ -32,6 +32,7 @@
 
 #include "project_defs.h"
 #include "func.h"
+#include "leds.h"
 
 // ----------------------------------------------------------------------------------------
 
@@ -133,7 +134,7 @@
     }
     else if ( error_rear < -k_max_motor_error_constant )
     {
-        adjust_motor_rear( k_max_motor_error_constant );
+        adjust_motor_rear( -k_max_motor_error_constant );
     }
     else
     {
@@ -142,8 +143,13 @@
 
     if ( error_front != 0 || error_rear != 0 )
     {
+        led_green_2_on();
         motorctl_set_speed( s_motor_front, s_motor_rear );
     }
+    else
+    {
+        led_green_2_off();
+    }
 }
 
 // ----------------------------------------------------------------------------------------