changeset 190:3a5b59c73b7f main

Improved cantool for steer, speed, and estop commands. Better pretty-printing for listen.
author Bob Cook <bob@bobcookdev.com>
date Thu, 13 Jun 2013 21:00:02 -0700
parents af5e626e4301
children e23198d929e9
files main/robots/cantool/CanToolApp.cpp main/robots/cantool/CmdEStop.cpp main/robots/cantool/CmdListen.cpp main/robots/cantool/CmdSteer.cpp main/robots/cantool/Commands.h main/robots/cantool/jamfile
diffstat 6 files changed, 201 insertions(+), 3 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/cantool/CanToolApp.cpp	Sat Jun 08 13:23:58 2013 -0700
+++ b/main/robots/cantool/CanToolApp.cpp	Thu Jun 13 21:00:02 2013 -0700
@@ -69,8 +69,10 @@
     loadConfiguration();
     Poco::Util::Application::initialize( self );
 
+    m_commandDispatchTable[ "estop" ] = CommandEStop;
     m_commandDispatchTable[ "listen" ] = CommandListen;
-    m_commandDispatchTable[ "motors" ] = CommandMotors;
+    m_commandDispatchTable[ "speed" ] = CommandSpeed;
+    m_commandDispatchTable[ "steer" ] = CommandSteer;
     m_commandDispatchTable[ "sonarfront" ] = CommandSonarFront;
 }
 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/CmdEStop.cpp	Thu Jun 13 21:00:02 2013 -0700
@@ -0,0 +1,78 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/cantool/CmdEStop.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Command "estop" implementation.
+//
+//  Copyright (c) 2011-2013 Bob Cook
+//
+//  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 <iostream>
+
+#include "Commands.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/linux/can/CANMessage.h"
+#include "packages/linux/can/CANMsgProcessor.h"
+
+// ----------------------------------------------------------------------------------------
+
+void CommandEStop( const std::vector< std::string >& args )
+{
+    if ( args.size() < 2 )
+    {
+        std::cerr << "error: estop <on|clear>" << std::endl;
+        return;
+    }
+
+    uint32_t msgid;
+
+    if ( args[ 1 ] == "on" )
+    {
+        msgid = can_build_message_id( can_node_odr_manager,
+                                      can_node_broadcast,
+                                      can_dataid_emergency );
+    }
+    else if ( args[ 1 ] == "clear" )
+    {
+        msgid = can_build_message_id( can_node_odr_manager,
+                                      can_node_broadcast,
+                                      can_dataid_all_clear );
+    }
+    else
+    {
+        std::cerr << "error: estop <on|clear>" << std::endl;
+        return;
+    }
+
+    CANMessage::QueueToSend( new CANMessage( msgid ) );
+    std::cout << "sent " << args[ 1 ] << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/cantool/CmdListen.cpp	Sat Jun 08 13:23:58 2013 -0700
+++ b/main/robots/cantool/CmdListen.cpp	Thu Jun 13 21:00:02 2013 -0700
@@ -63,9 +63,15 @@
         case can_node_odr_manager:
             return "odr-manager";
 
+        case can_node_odr_controller:
+            return "odr-controller";
+
         case can_node_odr_motion:
             return "odr-motion";
 
+        case can_node_odr_sonar_front:
+            return "odr-sonar-front";
+
         default:
             return "(unknown node)";
     }
@@ -119,6 +125,14 @@
 
         switch ( dataId )
         {
+            case can_dataid_emergency:
+                std::cout << "emergency from " << NodeName( srcNode ) << std::endl;
+                break;
+
+            case can_dataid_all_clear:
+                std::cout << "all-clear from " << NodeName( srcNode ) << std::endl;
+                break;
+
             case can_dataid_heartbeat:
                 std::cout << "heartbeat from " << NodeName( srcNode ) << std::endl;
                 break;
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/cantool/CmdSteer.cpp	Thu Jun 13 21:00:02 2013 -0700
@@ -0,0 +1,101 @@
+// ----------------------------------------------------------------------------------------
+//
+//  robots/cantool/CmdSteer.cpp
+//    
+//  Bob Cook Development, Robotics Library
+//  http://www.bobcookdev.com/rl/
+//
+//  Command "steer" implementation.
+//
+//  Copyright (c) 2013 Bob Cook
+//
+//  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 <iostream>
+
+#include <Poco/Exception.h>
+#include <Poco/NumberFormatter.h>
+#include <Poco/NumberParser.h>
+
+#include "Commands.h"
+
+#include "packages/common/can/can_helpers.h"
+#include "packages/common/can/can_messages.h"
+#include "packages/common/can/can_nodes.h"
+
+#include "packages/linux/can/CANMessage.h"
+#include "packages/linux/can/CANMsgProcessor.h"
+
+// ----------------------------------------------------------------------------------------
+
+static void PrintHelp()
+{
+    std::cerr << "error: steer <front_angle> <rear_angle>" << std::endl;
+    std::cerr << "       where \"angle\" is specified as [-12,12]" << std::endl;
+    std::cerr << "       (enter negative values as: \"steer -- -10 -10\"" << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
+void CommandSteer( const std::vector< std::string >& args )
+{
+    if ( args.size() < 3 )
+    {
+        PrintHelp();
+        return;
+    }
+
+    uint32_t msgid = can_build_message_id( can_node_odr_manager,
+                                           can_node_odr_motion,
+                                           can_dataid_wheel_position );
+    can_data_wheel_position msg;
+
+    try
+    {
+        int front = Poco::NumberParser::parse( args[ 1 ] );
+        int rear  = Poco::NumberParser::parse( args[ 2 ] );
+
+        if ( front < -12 || front > 12 || rear < -12 || rear > 12 )
+        {
+            throw Poco::SyntaxException( "value out of range", 1 );
+        }
+
+        msg.angle_front = front;
+        msg.angle_rear  = rear;
+    }
+    catch ( const Poco::SyntaxException& ex )
+    {
+        PrintHelp();
+        return;
+    }
+
+    CANMessage::QueueToSend(
+        new CANMessage( msgid, reinterpret_cast< uint8_t* >( &msg ), sizeof( msg ) ) );
+
+    std::cout << "sent "
+        << Poco::NumberFormatter::format( msg.angle_front )
+        << "," 
+        << Poco::NumberFormatter::format( msg.angle_rear ) 
+        << std::endl;
+}
+
+// ----------------------------------------------------------------------------------------
+
--- a/main/robots/cantool/Commands.h	Sat Jun 08 13:23:58 2013 -0700
+++ b/main/robots/cantool/Commands.h	Thu Jun 13 21:00:02 2013 -0700
@@ -34,9 +34,12 @@
 
 // ----------------------------------------------------------------------------------------
 
+void CommandEStop( const std::vector< std::string >& args );
+
 void CommandListen( const std::vector< std::string >& args );
 
-void CommandMotors( const std::vector< std::string >& args );
+void CommandSpeed( const std::vector< std::string >& args );
+void CommandSteer( const std::vector< std::string >& args );
 
 void CommandSonarFront( const std::vector< std::string >& args );
 
--- a/main/robots/cantool/jamfile	Sat Jun 08 13:23:58 2013 -0700
+++ b/main/robots/cantool/jamfile	Thu Jun 13 21:00:02 2013 -0700
@@ -37,7 +37,7 @@
 COMMON_SOURCES = 
     main.cpp
     CanToolApp.cpp
-    CmdListen.cpp CmdMotors.cpp CmdSonarFront.cpp
+    CmdEStop.cpp CmdListen.cpp CmdSpeed.cpp CmdSonarFront.cpp CmdSteer.cpp
     packages.common.can.pkg
     packages.linux.can.pkg
     ;