Revision: 9005
http://playerstage.svn.sourceforge.net/playerstage/?rev=9005&view=rev
Author: jpgr87
Date: 2010-12-11 21:51:30 +0000 (Sat, 11 Dec 2010)
Log Message:
-----------
Applied patch #3134435: Lego NXT driver
Modified Paths:
--------------
code/player/trunk/server/drivers/mixed/CMakeLists.txt
Added Paths:
-----------
code/player/trunk/server/drivers/mixed/nxt/
code/player/trunk/server/drivers/mixed/nxt/CMakeLists.txt
code/player/trunk/server/drivers/mixed/nxt/examples/
code/player/trunk/server/drivers/mixed/nxt/examples/standalone.cc
code/player/trunk/server/drivers/mixed/nxt/src/
code/player/trunk/server/drivers/mixed/nxt/src/chronos.cc
code/player/trunk/server/drivers/mixed/nxt/src/chronos.hh
code/player/trunk/server/drivers/mixed/nxt/src/nxt_driver.cc
code/player/trunk/server/drivers/mixed/nxt/src/nxtdc.cc
code/player/trunk/server/drivers/mixed/nxt/src/nxtdc.hh
Modified: code/player/trunk/server/drivers/mixed/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/mixed/CMakeLists.txt 2010-12-11
21:25:28 UTC (rev 9004)
+++ code/player/trunk/server/drivers/mixed/CMakeLists.txt 2010-12-11
21:51:30 UTC (rev 9005)
@@ -12,6 +12,7 @@
ADD_SUBDIRECTORY (mbase)
ADD_SUBDIRECTORY (mricp)
ADD_SUBDIRECTORY (nomad)
+ADD_SUBDIRECTORY (nxt)
ADD_SUBDIRECTORY (p2os)
ADD_SUBDIRECTORY (phidgetIFK)
ADD_SUBDIRECTORY (reb)
Added: code/player/trunk/server/drivers/mixed/nxt/CMakeLists.txt
===================================================================
--- code/player/trunk/server/drivers/mixed/nxt/CMakeLists.txt
(rev 0)
+++ code/player/trunk/server/drivers/mixed/nxt/CMakeLists.txt 2010-12-11
21:51:30 UTC (rev 9005)
@@ -0,0 +1,19 @@
+PLAYERDRIVER_OPTION (nxt build_nxt ON)
+PLAYERDRIVER_REQUIRE_PKG (nxt build_nxt libusb-1.0
+ nxt_includeDirs nxt_libDirs nxt_linkLibs
+ nxt_linkFlags nxt_cFlags)
+
+if (NOT HAVE_STL)
+ PLAYERDRIVER_OPTION (nxt build_nxt OFF "STL not found")
+endif(NOT HAVE_STL)
+
+PLAYERDRIVER_ADD_DRIVER (nxt build_nxt
+ INCLUDEDIRS ${nxt_includeDirs} LIBDIRS ${nxt_libDirs}
+ LINKLIBS ${nxt_linkLibs} LINKFLAGS ${nxt_linkFlags}
+ CFLAGS ${nxt_cFlags}
+ SOURCES
+ src/chronos.cc
+ src/nxt_driver.cc
+ src/nxtdc.cc
+)
+
Added: code/player/trunk/server/drivers/mixed/nxt/examples/standalone.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/nxt/examples/standalone.cc
(rev 0)
+++ code/player/trunk/server/drivers/mixed/nxt/examples/standalone.cc
2010-12-11 21:51:30 UTC (rev 9005)
@@ -0,0 +1,72 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Alejandro R. Mosteo
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <cstdio>
+#include "nxtdc.hh"
+#include <unistd.h>
+
+using namespace NXT;
+using namespace std;
+
+void set_power ( brick &b, int power )
+{
+ const int i = power;
+
+ b.set_motor ( B, i );
+ b.set_motor ( C, -i );
+
+ usleep ( 100000 );
+ const output_state st_b = b.get_motor_state ( B );
+ const output_state st_c = b.get_motor_state ( C );
+ printf ( "Power: %4d B:[%6d/%6d/%6d/%6d] C:[%6d/%6d/%6d/%6d]\n",
+ i,
+ st_b.tacho_limit, st_b.tacho_count, st_b.block_tacho_count,
st_b.rotation_count,
+ st_c.tacho_limit, st_c.tacho_count, st_c.block_tacho_count,
st_c.rotation_count );
+}
+
+int main ( void )
+{
+ NXT::brick b;
+
+ const NXT::versions v = b.get_version();
+
+ printf ( "Connected to NXT brick named %s, protocol %d.%d firmware %d.%d\n",
+ b.get_device_info().brick_name,
+ v.protocol_major, v.protocol_minor,
+ v.firmware_major, v.firmware_minor );
+
+ printf ( "Battery: %d\n", b.get_battery_level() );
+
+ b.play_tone ( 700, 1000 );
+
+ for ( int i = 0; i <= 100; i++ )
+ set_power ( b, i );
+ for ( int i = 100; i >= -100; i-- )
+ set_power ( b, i );
+ for ( int i = -100; i <= 0; i++ )
+ set_power ( b, i );
+
+ printf ( "Battery: %d\n", b.get_battery_level() );
+
+ // b.msg_rate_check();
+
+ return 0;
+}
Added: code/player/trunk/server/drivers/mixed/nxt/src/chronos.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/nxt/src/chronos.cc
(rev 0)
+++ code/player/trunk/server/drivers/mixed/nxt/src/chronos.cc 2010-12-11
21:51:30 UTC (rev 9005)
@@ -0,0 +1,55 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Alejandro R. Mosteo
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <cerrno>
+#include "chronos.hh"
+#include <cstring>
+#include <stdexcept>
+#include <sys/time.h>
+
+using namespace nxt_driver;
+
+Chronos::Chronos ( double seconds_since_epoch )
+{
+ clock_ = seconds_since_epoch;
+}
+
+double Chronos::elapsed ( void ) const
+ {
+ return now() - clock_;
+ }
+
+void Chronos::reset ( void )
+{
+ clock_ = now();
+}
+
+double Chronos::now ( void )
+ {
+ struct timeval clock;
+
+ if ( gettimeofday ( &clock, NULL ) != 0 )
+ throw std::runtime_error ( strerror ( errno ) );
+
+ return
+ static_cast<double> ( clock.tv_sec ) +
+ static_cast<double> ( clock.tv_usec ) * 1e-6;
+ }
\ No newline at end of file
Added: code/player/trunk/server/drivers/mixed/nxt/src/chronos.hh
===================================================================
--- code/player/trunk/server/drivers/mixed/nxt/src/chronos.hh
(rev 0)
+++ code/player/trunk/server/drivers/mixed/nxt/src/chronos.hh 2010-12-11
21:51:30 UTC (rev 9005)
@@ -0,0 +1,43 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Alejandro R. Mosteo
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef _chronos_
+#define _chronos_
+
+namespace nxt_driver
+ {
+
+ class Chronos
+ {
+ public:
+ Chronos ( double seconds_since_epoch = now() );
+ double elapsed ( void ) const;
+ void reset ( void );
+
+ private:
+ double clock_;
+
+ static double now ( void ) ;
+ };
+
+}
+
+#endif
Added: code/player/trunk/server/drivers/mixed/nxt/src/nxt_driver.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/nxt/src/nxt_driver.cc
(rev 0)
+++ code/player/trunk/server/drivers/mixed/nxt/src/nxt_driver.cc
2010-12-11 21:51:30 UTC (rev 9005)
@@ -0,0 +1,412 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Alejandro R. Mosteo
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+/** @ingroup drivers */
+/** @{ */
+/** @defgroup driver_nxt nxt
+ * @brief Lego Mindstorms NXT
+
+This driver implements partial interaction with a USB-connected Lego
Mindstorms NXT brick.\n
+Motors are implemented.\n
+Sensors are unimplemented.
+
+...@par Compile-time dependencies
+
+- libusb-1.0 or newer (www.libusb.org)
+
+...@par Provides
+
+- @ref interface_position1d
+ - One per each of the A, B, C motors
+ - These can be aggregated in a position2d using, e.g., @ref
driver_differential
+ - Velocity commands are accepted. Position commands are not.
+- @ref interface_power
+ - Battery level of the brick.
+
+...@par Configuration file options
+
+- max_power (tuple of float [%] default: [100 100 100])
+ - Power applied when maximum vel is requested for each motor.
+
+- max_speed (tuple of float [length/s] default: [0.5 0.5 0.5])
+ - Speed that each motor provides at max_power (must be calibrated/measured
somehow depending on the LEGO model built).
+
+- odom_rate (tuple of float default [0.0005 0.0005 0.0005])
+ - Multiplier for the tachometer in the lego motor. tacho_count x odom_rate =
real_distance (must be calibrated also).
+ - Default is somewhat close to the standard small wheels with direct motor
drive.
+
+- period (float [s] default 0.05)
+ - Seconds between reads of motor encoders. Since this requires polling and
affects CPU use, each app can set an adequate timing.
+ - Note that a polling roundtrip via USB takes (empirically measured) around
2ms per motor.
+
+...@par Example
+
+...@verbatim
+
+# Standard configured brick with B and C motors in use
+
+unit_length "m"
+unit_angle "radians"
+
+# The NXT driver
+driver
+(
+ name "nxt"
+ provides [ "B:::position1d:0" "C:::position1d:1" "power:0" ]
+
+ max_power [100 100 100] # 100% power is to be used
+ max_speed [0.5 0.5 0.5] # in order to achieve 0.5 m/s linearly
+ odom_rate [0.1 0.1 0.1] # multiplier for odometry
+
+ period 0.05
+)
+
+# The differential driver that provides simplified position2d management
+driver
+(
+ name "differential"
+ requires [ "left:::position1d:0" "right:::position1d:1" ]
+ provides [ "position2d:0" ]
+
+ axis_length 0.25
+)
+
+...@endverbatim
+
+...@author Alejandro R. Mosteo
+*/
+/** @} */
+
+#include "chronos.hh"
+#include "libplayercore/playercore.h"
+#include "nxtdc.hh"
+
+using namespace nxt_driver;
+
+const int kNumMotors = 3;
+
+class Nxt : public ThreadedDriver
+ {
+ public:
+ Nxt ( ConfigFile* cf, int section );
+
+ virtual void Main ( void );
+ virtual int MainSetup ( void );
+ virtual void MainQuit ( void );
+
+ virtual int ProcessMessage ( QueuePointer &resp_queue, player_msghdr *
hdr, void * data );
+
+ private:
+ player_devaddr_t motor_addr_[kNumMotors];
+ player_devaddr_t power_addr_;
+
+ player_position1d_data_t data_state_ [kNumMotors]; // Just read status.
+ player_position1d_data_t data_state_prev_[kNumMotors]; // Previous status
to integrate speed.
+
+ double max_power_[kNumMotors];
+ double max_speed_[kNumMotors];
+ double odom_rate_[kNumMotors];
+
+ bool publish_motor_[kNumMotors];
+ bool publish_power_;
+
+ player_power_data_t juice_;
+
+ double period_;
+ Chronos timer_battery_;
+ Chronos timer_period_;
+
+ NXT::brick *brick_;
+
+ void CheckBattery ( void );
+ void CheckMotors ( void );
+ NXT::motors GetMotor ( const player_devaddr_t &addr ) const;
+ int8_t GetPower ( float vel, NXT::motors motor ) const;
+ };
+
+Driver* nxt_Init ( ConfigFile* cf, int section )
+{
+ return static_cast<Driver*> ( new Nxt ( cf, section ) );
+}
+
+void nxt_Register ( DriverTable* table )
+{
+ table->AddDriver ( "nxt", nxt_Init );
+}
+
+const char *motor_names[kNumMotors] = { "A", "B", "C" };
+
+Nxt::Nxt ( ConfigFile *cf, int section )
+ : ThreadedDriver ( cf, section ),
+ period_ ( cf->ReadFloat ( section, "period", 0.05 ) ),
+ timer_battery_ ( -666.0 ) // Ensure first update to be sent immediately
+{
+ for ( int i = 0; i < kNumMotors; i++ )
+ {
+ // Read them regardless of motor usage to placate player unused warnings
+ max_power_[i] = cf->ReadTupleFloat ( section, "max_power", i, 100.0 );
+ max_speed_[i] = cf->ReadTupleFloat ( section, "max_speed", i, 0.5 );
+ odom_rate_[i] = cf->ReadTupleFloat ( section, "odom_rate", i, 0.0005 );
+
+ if ( cf->ReadDeviceAddr ( &motor_addr_[i], section, "provides",
PLAYER_POSITION1D_CODE, -1, motor_names[i] ) == 0 )
+ {
+ PLAYER_MSG1 ( 3, "nxt: Providing motor %s", motor_names[i] );
+
+ if ( AddInterface ( motor_addr_[i] ) != 0 )
+ throw std::runtime_error ( "Cannot add position1d interface" );
+ else
+ {
+ publish_motor_[i] = true;
+
+ data_state_[i].pos = 0.0f;
+ data_state_[i].vel = 0.0f;
+ data_state_[i].stall = 0;
+ data_state_[i].status = ( 1 << PLAYER_POSITION1D_STATUS_ENABLED
);
+ }
+ }
+ }
+
+ if ( cf->ReadDeviceAddr ( &power_addr_, section, "provides",
PLAYER_POWER_CODE, -1, NULL ) == 0 )
+ {
+ if ( AddInterface ( power_addr_ ) != 0 )
+ throw std::runtime_error ( "Cannot add power interface" );
+ else
+ publish_power_ = true;
+ }
+ else
+ {
+ publish_power_ = false;
+ }
+
+}
+
+int Nxt::MainSetup ( void )
+{
+ brick_ = new NXT::brick();
+
+ // Reset odometries to origin
+ for ( int i = 0; i < kNumMotors; i++ )
+ if ( publish_motor_[i] )
+ brick_->execute ( brick_->prepare_reset_motor_position (
static_cast<NXT::motors> ( i ), false ) );
+
+ return 0;
+}
+
+void Nxt::MainQuit ( void )
+{
+ // Stop motors just in case they're running.
+ // The brick has no watchdog, so they will keep its last commanded speed
forever
+ for ( int i = 0; i < kNumMotors; i++ )
+ if ( publish_motor_[i] )
+ brick_->set_motor ( static_cast<NXT::motors> ( i ), 0 );
+
+ delete brick_;
+}
+
+void Nxt::Main ( void )
+{
+ while ( true )
+ {
+ // Wait till we get new data or we need to measure something
+ Wait ( period_ );
+
+ pthread_testcancel();
+
+ ProcessMessages ( 0 );
+
+ CheckBattery();
+ CheckMotors();
+ }
+}
+
+void Nxt::CheckBattery ( void )
+{
+ if ( ! publish_power_ )
+ return;
+
+ // We don't want to poll battery level innecesarily often (how much is this,
really?)
+ if ( timer_battery_.elapsed() > 10.0 )
+ {
+ timer_battery_.reset();
+
+ juice_.valid = PLAYER_POWER_MASK_VOLTS;
+ juice_.volts = static_cast<float> ( brick_->get_battery_level() ) /
1000.0f;
+ // Omitted 4 unknown values here
+ };
+
+ // Publish value
+ if ( HasSubscriptions() )
+ Publish ( power_addr_,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_POWER_DATA_STATE,
+ static_cast<void*> ( &juice_ ) );
+
+ PLAYER_MSG1 ( 3, "Publishing power: %8.2f\n", juice_.volts );
+}
+
+void Nxt::CheckMotors ( void )
+{
+ if ( timer_period_.elapsed() < period_ )
+ return;
+
+ timer_period_.reset();
+
+ // First we get odometry updates from brick
+ for ( int i = 0; i < kNumMotors; i++ )
+ {
+ if ( ! publish_motor_[i] )
+ continue;
+
+ const NXT::output_state state = brick_->get_motor_state (
static_cast<NXT::motors> ( i ) );
+
+ data_state_[i].pos = state.tacho_count * odom_rate_[i];
+ data_state_[i].vel = ( data_state_[i].pos - data_state_prev_[i].pos ) /
period_;
+
+ PLAYER_MSG3 ( 5, "nxt: odom read is [raw/adjusted/vel] = [ %8d / %8.2f /
%8.2f ]",
+ state.tacho_count, data_state_[i].pos, data_state_[i].vel
);
+
+ data_state_prev_[i] = data_state_[i];
+ }
+
+ // Then we publish them together, to minimize unsyncing in a consuming
driver (e.g. differential driver)
+ for ( int i = 0; i < kNumMotors; i++ )
+ if ( publish_motor_[i] && HasSubscriptions() )
+ Publish ( motor_addr_[i],
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_POSITION1D_DATA_STATE,
+ static_cast<void*> ( &data_state_[i] ) );
+
+}
+
+int Nxt::ProcessMessage ( QueuePointer & resp_queue,
+ player_msghdr * hdr,
+ void * data )
+{
+ if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POWER_REQ_SET_CHARGING_POLICY, power_addr_ ) )
+ {
+ PLAYER_WARN ( "nxt: there are no charging policies." );
+ return 0;
+ }
+
+ if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION1D_CMD_POS ) ||
+ Message::MatchMessage ( hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION1D_REQ_POSITION_PID ) )
+ {
+ PLAYER_WARN ( "nxt: position commands not supported" );
+ return 0;
+ }
+
+ if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_CMD,
PLAYER_POSITION1D_CMD_VEL ) )
+ {
+ player_position1d_cmd_vel_t &vel =
*static_cast<player_position1d_cmd_vel_t*> ( data );
+
+ const NXT::motors motor = GetMotor ( hdr->addr );
+ brick_->set_motor ( motor, GetPower ( vel.vel, motor ) );
+
+ return 0;
+ }
+
+ if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION1D_REQ_GET_GEOM ) )
+ {
+ PLAYER_WARN ( "nxt: geometry not supported" );
+ return 0;
+ }
+
+ if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION1D_REQ_POSITION_MODE ) )
+ {
+ PLAYER_WARN ( "nxt: mode is always speed" );
+ return 0;
+ }
+
+ if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION1D_REQ_MOTOR_POWER ) )
+ {
+ PLAYER_WARN ( "nxt: motors are always on" );
+ return 0;
+ }
+
+ if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION1D_REQ_RESET_ODOM ) )
+ {
+ brick_->execute ( brick_->prepare_reset_motor_position ( GetMotor (
hdr->addr ) ) );
+ return 0;
+ }
+
+ if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION1D_REQ_SET_ODOM ) )
+ {
+ PLAYER_WARN ( "nxt: odometry setting to arbitrary values not supported"
);
+ return 0;
+ }
+
+ if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION1D_REQ_SPEED_PID ) )
+ {
+ PLAYER_WARN ( "nxt: speed profiles not supported" );
+ return 0;
+ }
+
+ if ( Message::MatchMessage ( hdr, PLAYER_MSGTYPE_REQ,
PLAYER_POSITION1D_REQ_SPEED_PROF ) )
+ {
+ PLAYER_WARN ( "nxt: acceleration ignored, adjusting max speed only" );
+ const NXT::motors motor = GetMotor ( hdr->addr );
+ const player_position1d_speed_prof_req_t &prof =
*static_cast<player_position1d_speed_prof_req_t*> ( data );
+
+ max_power_[motor] *= ( prof.speed / max_speed_[motor] ); // Adjust power
proportionally
+ max_speed_[motor] = prof.speed;
+
+ if ( abs ( max_power_[motor] ) > 100 )
+ PLAYER_WARN2 ( "nxt: requested speed would require excess power:
[speed/power] = [ %8.2f / %8.2f ]",
+ max_speed_[motor], max_power_[motor] );
+ return 0;
+ }
+
+ PLAYER_WARN4 ( "nxt: Message not processed idx:%d type:%d sub:%d seq:%d\n",
hdr->addr.index, hdr->type, hdr->subtype, hdr->seq );
+ return -1;
+}
+
+
+NXT::motors Nxt::GetMotor ( const player_devaddr_t &addr ) const
+ {
+ for ( int i = 0; i < kNumMotors; i++ )
+ if ( ( addr.host == motor_addr_[i].host ) &&
+ ( addr.robot == motor_addr_[i].robot ) &&
+ ( addr.index == motor_addr_[i].index ) &&
+ ( addr.interf == motor_addr_[i].interf ) )
+ return static_cast<NXT::motors> ( i );
+
+ throw std::runtime_error ( "nxt: received request for unknown motor" );
+ }
+
+template<class T>
+T sign ( T x )
+{
+ return x >= 0 ? 1 : -1;
+}
+
+int8_t Nxt::GetPower ( float vel, NXT::motors motor ) const
+{
+ const int8_t power = vel / max_speed_[motor] * max_power_[motor];
+ if ( abs ( power ) > abs ( max_power_[motor] ) )
+ {
+ PLAYER_WARN3 ( "nxt: exceeded max power [motor/reqvel/reqpwr] = [ %s /
%8.2f / %d ]",
+ motor_names[motor], vel, power );
+ return max_power_[motor] * sign<float> ( power );
+ }
+ else
+ return power;
+}
+
Added: code/player/trunk/server/drivers/mixed/nxt/src/nxtdc.cc
===================================================================
--- code/player/trunk/server/drivers/mixed/nxt/src/nxtdc.cc
(rev 0)
+++ code/player/trunk/server/drivers/mixed/nxt/src/nxtdc.cc 2010-12-11
21:51:30 UTC (rev 9005)
@@ -0,0 +1,494 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Alejandro R. Mosteo
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <cassert>
+#include <cstdio>
+#include <cstring>
+#include <endian.h>
+#include "nxtdc.hh"
+#include <sstream>
+#include <sys/time.h>
+
+using namespace NXT;
+using namespace std;
+
+const uint16_t VENDOR_LEGO = 0x0694;
+const uint16_t PRODUCT_NXT = 0x0002;
+
+const int kNxtConfig = 1;
+const int kNxtInterface = 0;
+
+const uint8_t kMaxTelegramSize = 64; // Per NXT spec.
+
+const unsigned char kOutEndpoint = 0x1;
+const unsigned char kInEndpoint = 0x82;
+
+enum direct_commands
+{
+ command_play_tone = 0x03,
+ command_set_output_state = 0x04,
+ command_get_output_state = 0x06,
+ command_reset_motor_position = 0x0A,
+ command_get_battery_level = 0x0B,
+ command_stop_sound_playback = 0x0C,
+ command_keep_alive = 0x0D
+};
+
+enum system_commands
+{
+ system_get_firmware_version = 0x88,
+ system_get_device_info = 0x9B
+};
+
+const char *usberr_to_str ( int err )
+{
+ switch ( err )
+ {
+ case 0:
+ return "LIBUSB_SUCCESS";
+ case -1:
+ return "LIBUSB_ERROR_IO";
+ case -2:
+ return "LIBUSB_ERROR_INVALID_PARAM";
+ case -3:
+ return "LIBUSB_ERROR_ACCESS";
+ case -4:
+ return "LIBUSB_ERROR_NO_DEVICE";
+ case -5:
+ return "LIBUSB_ERROR_NOT_FOUND";
+ case -6:
+ return "LIBUSB_ERROR_BUSY";
+ case -7:
+ return "LIBUSB_ERROR_TIMEOUT";
+ case -8:
+ return "LIBUSB_ERROR_OVERFLOW";
+ case -9:
+ return "LIBUSB_ERROR_PIPE";
+ case -10:
+ return "LIBUSB_ERROR_INTERRUPTED";
+ case -11:
+ return "LIBUSB_ERROR_NO_MEM";
+ case -12:
+ return "LIBUSB_ERROR_NOT_SUPPORTED";
+ case -99:
+ return "LIBUSB_ERROR_OTHER";
+ default:
+ return "LIBUSB_ERROR_UNKNOWN";
+ }
+}
+
+string nxterr_to_str ( int err )
+{
+ switch ( err )
+ {
+ case 0x20:
+ return "Pending communication transaction in progress";
+ case 0x40:
+ return "Specified mailbox queue is empty";
+ case 0xBD:
+ return "Request failed (e.g. specified file not found)";
+ case 0xBE:
+ return "Unknown command opcode";
+ case 0xBF:
+ return "Insane packet";
+ case 0xC0:
+ return "Data contains out-of-range values";
+ case 0xDD:
+ return "Communication bus error";
+ case 0xDE:
+ return "No free memory in communication buffer";
+ case 0xDF:
+ return "Specified channel/connection is not valid";
+ case 0xE0:
+ return "Specified channel/connection not configured or busy";
+ case 0xEC:
+ return "No active program";
+ case 0xED:
+ return "Illegal size specified";
+ case 0xEE:
+ return "Illegal mailbox queue ID specified";
+ case 0xEF:
+ return "Attempted to access invalid field or structure";
+ case 0xF0:
+ return "Bad input or output specified";
+ case 0xFB:
+ return "Insufficient memory available";
+ case 0xFF:
+ return "Bad arguments";
+ default:
+ char s[100];
+ snprintf ( s, 100, "NXT_UNCATEGORIZED_ERROR: 0x%02x", err );
+ return string ( s );
+ }
+}
+
+buffer & buffer::append_byte ( uint8_t byte )
+{
+ push_back ( static_cast<unsigned char> ( byte ) );
+ return *this;
+}
+
+buffer & buffer::append_word ( uint16_t word )
+{
+ union
+ {
+ unsigned char bytes[2];
+ uint16_t le_word;
+ } tmp;
+
+ tmp.le_word = htole16 ( word );
+
+ push_back ( tmp.bytes[0] );
+ push_back ( tmp.bytes[1] );
+
+ return *this;
+}
+
+buffer & buffer::append ( const buffer & buf )
+{
+ for ( size_t i = 0; i < buf.size(); i++ )
+ push_back ( buf[i] );
+
+ return *this;
+}
+
+void buffer::dump ( const string & header ) const
+ {
+ printf ( "%s\n", header.c_str() );
+
+ for ( size_type i = 0; i < this->size(); i++ )
+ printf ( "%2d = 0x%02x\n", i, this->at ( i ) );
+ }
+
+void USB_transport::usb_check ( int usb_error )
+{
+ if ( usb_error != LIBUSB_SUCCESS )
+ throw runtime_error ( string ( "USB error: " ) + usberr_to_str ( usb_error
) );
+}
+
+USB_transport::USB_transport ( void )
+{
+ usb_check ( libusb_init ( &context_ ) );
+ libusb_set_debug ( context_, 3 );
+
+ handle_ = libusb_open_device_with_vid_pid ( context_, VENDOR_LEGO,
PRODUCT_NXT );
+ if ( handle_ == NULL )
+ throw runtime_error ( "USB_transport: brick not found." );
+
+ usb_check ( libusb_set_configuration ( handle_, kNxtConfig ) );
+ usb_check ( libusb_claim_interface ( handle_, kNxtInterface ) );
+ usb_check ( libusb_reset_device ( handle_ ) );
+}
+
+USB_transport::~USB_transport ( void )
+{
+ // usb_check(libusb_release_interface(handle_, kNxtInterface));
+ // Fails, why?
+
+ libusb_close ( handle_ );
+ libusb_exit ( context_ );
+}
+
+void USB_transport::write ( const buffer &buf )
+{
+ int transferred;
+
+ // buf.dump ( "write" );
+
+ usb_check ( libusb_bulk_transfer
+ ( handle_, kOutEndpoint,
+ ( unsigned char* ) &buf[0], buf.size(),
+ &transferred, 0 ) );
+ // printf ( "T:%d\n", transferred );
+}
+
+buffer USB_transport::read ( void )
+{
+ unsigned char buf[kMaxTelegramSize];
+ int transferred;
+
+ usb_check ( libusb_bulk_transfer
+ ( handle_, kInEndpoint,
+ &buf[0], kMaxTelegramSize,
+ &transferred, 0 ) );
+ // printf ( "%2x %2x %2x (%d read)\n", buf[0], buf[1], buf[2], transferred );
+
+ buffer result;
+ result.reserve ( transferred );
+
+ for ( int i = 0; i < transferred; i++ )
+ result.push_back ( buf[i] );
+
+ return result ;
+}
+
+brick::brick ( void )
+{
+ ;
+}
+
+brick::~brick ( void )
+{
+ ;
+}
+
+buffer brick::execute ( const buffer &command, bool with_feedback )
+{
+ assert ( command.size() >= 2 );
+
+ if ( with_feedback && ( ! ( command[0] & 0x80 ) ) )
+ link_.write ( command );
+ else if ( ( !with_feedback ) && ( command[0] & 0x80 ) )
+ link_.write ( command );
+ else
+ {
+ buffer newcomm ( command );
+
+ // Set or reset 0x80 bit (confirmation request)
+ newcomm[0] = ( with_feedback ? command[0] & 0x7F : command[0] | 0x80 );
+
+ link_.write ( newcomm );
+ }
+
+ if ( with_feedback )
+ {
+ const buffer reply = link_.read ();
+ if ( reply.size() < 3 )
+ {
+ stringstream s;
+ s << "Reply too short: " << reply.size() << " bytes";
+ throw nxt_error ( s.str () );
+ }
+ else if ( reply[0] != brick::reply )
+ {
+ char s[100];
+ snprintf ( s, 100, "Unexpected telegram: 0x%02x != 0x%02x",
reply[0], brick::reply );
+ throw nxt_error ( s );
+ }
+ else if ( reply[1] != command[1] )
+ {
+ char s[100];
+ snprintf ( s, 100, "Unexpected reply type: 0x%02x != 0x%02x",
reply[1], command[1] );
+ throw nxt_error ( s );
+ }
+ else if ( reply[2] != 0 )
+ throw nxt_error ( nxterr_to_str ( reply[2] ) );
+ else
+ return reply;
+ }
+ else
+ return buffer();
+}
+
+buffer brick::prepare_play_tone ( uint16_t tone_Hz, uint16_t duration_ms )
+{
+ return assemble ( direct_command_without_response,
+ command_play_tone,
+ buffer().
+ append_word ( tone_Hz ).
+ append_word ( duration_ms ) ) ;
+}
+
+buffer brick::prepare_output_state (
+ motors motor ,
+ int8_t power_pct ,
+ motor_modes mode ,
+ regulation_modes regulation ,
+ int8_t turn_ratio ,
+ motor_run_states state ,
+ uint32_t tacho_count )
+{
+ union
+ {
+ uint8_t bytes[4];
+ uint32_t tacho;
+ } aux;
+
+ aux.tacho = htole32 ( tacho_count );
+
+ return
+ assemble
+ ( direct_command_without_response,
+ command_set_output_state,
+ buffer().
+ append_byte ( motor ).
+ append_byte ( power_pct ).
+ append_byte ( mode ).
+ append_byte ( regulation ).
+ append_byte ( turn_ratio ).
+ append_byte ( state ).
+ append_byte ( aux.bytes[0] ).append_byte ( aux.bytes[1] ).
+ append_byte ( aux.bytes[2] ).append_byte ( aux.bytes[3] ) );
+}
+
+buffer brick::prepare_reset_motor_position ( motors motor, bool
relative_to_last_position )
+{
+ return
+ assemble ( direct_command_without_response,
+ command_reset_motor_position,
+ buffer().
+ append_byte ( motor ).
+ append_byte ( relative_to_last_position ) );
+}
+
+buffer brick::prepare_stop_sound_playback ( void )
+{
+ return
+ assemble ( direct_command_without_response,
+ command_stop_sound_playback,
+ buffer() );
+}
+
+buffer brick::prepare_keep_alive ( void )
+{
+ return
+ assemble ( direct_command_without_response,
+ command_keep_alive,
+ buffer() );
+}
+
+void brick::play_tone ( uint16_t tone_Hz, uint16_t duration_ms )
+{
+ execute ( prepare_play_tone ( tone_Hz, duration_ms ), false );
+}
+
+void brick::set_motor ( motors motor, int8_t power_pct )
+{
+ execute
+ ( assemble
+ ( direct_command_without_response,
+ command_set_output_state,
+ buffer().
+ append_byte ( motor ).
+ append_byte ( power_pct ).
+ append_byte ( motor_brake ). // Brake uses a bit more power
but gives finer control at low speeds
+ append_byte ( regulation_motor_speed ). // Better Idle than Running,
which tries to compensate loads?
+ append_byte ( 0 ). // TURN_RATIO
+ append_byte ( power_pct == 0 ? motor_run_state_idle :
motor_run_state_running ).
+ append_word ( 0 ).append_word ( 0 ) ), // Tacho count (unlimited)
+ false );
+}
+
+output_state brick::get_motor_state ( motors motor )
+{
+ const buffer reply =
+ execute
+ ( assemble
+ ( direct_command_with_response,
+ command_get_output_state,
+ buffer().append_byte ( motor ) ) , true );
+
+ const output_state state =
+ {
+ reply[3], // motor
+ reply[4], // power_pct
+ static_cast<motor_modes> ( reply[5] ), // motor_modes
+ static_cast<regulation_modes> ( reply[6] ), // regulation_modes
+ reply[7], // turn_ratio
+ static_cast<motor_run_states> ( reply[8] ), // motor_run_states
+ le32toh ( *reinterpret_cast<const int32_t*> ( &reply[9] ) ),
+ le32toh ( *reinterpret_cast<const int32_t*> ( &reply[13] ) ),
+ le32toh ( *reinterpret_cast<const int32_t*> ( &reply[17] ) ),
+ le32toh ( *reinterpret_cast<const int32_t*> ( &reply[21] ) ),
+ };
+ return state;
+
+}
+
+versions brick::get_version ( void )
+{
+ const buffer reply =
+ execute
+ ( assemble
+ ( system_command_with_response,
+ system_get_firmware_version,
+ buffer() ) , true );
+
+ const versions v = { reply.at ( 3 ), reply.at ( 4 ), reply.at ( 5 ),
reply.at ( 6 ) };
+ return v;
+}
+
+device_info brick::get_device_info ( void )
+{
+ const buffer reply =
+ execute
+ ( assemble
+ ( system_command_with_response,
+ system_get_device_info,
+ buffer() ) , true );
+
+ device_info info;
+ strncpy ( info.brick_name, reinterpret_cast<const char*> ( &reply[3] ), 14 );
+ info.brick_name[14] = '\0';
+ strncpy ( info.bluetooth_address, reinterpret_cast<const char*> ( &reply[18]
), 6 );
+ info.bluetooth_address[6] = '\0';
+
+ return info;
+}
+
+uint16_t brick::get_battery_level ( void )
+{
+ const buffer reply =
+ execute ( assemble ( direct_command_with_response,
+ command_get_battery_level,
+ buffer() ),
+ true );
+
+ union
+ { const unsigned char * bytes;
+ const uint16_t * level;
+ } aux;
+
+ aux.bytes = &reply[3];
+ return le16toh ( *aux.level );
+}
+
+void brick::msg_rate_check ( void )
+{
+ struct timeval start, now;
+ int calls=0;
+
+ assert ( gettimeofday ( &start, NULL ) == 0 );
+
+ do
+ {
+ execute ( prepare_play_tone ( 440, 0 ), true );
+ calls++;
+
+ assert ( gettimeofday ( &now, NULL ) == 0 );
+ }
+ while ( ( double ) start.tv_sec + ( double ) start.tv_usec/1000000.0 -
+ ( double ) now.tv_sec - ( double ) now.tv_usec/1000000.0 > -10.0 );
+
+ printf ( "%d calls in 10s (%dms per call)\n", calls, 10000 / calls );
+}
+
+buffer brick::assemble ( telegram_types teltype,
+ uint8_t command,
+ const buffer & payload )
+{
+ return
+ buffer ().
+ append_byte ( teltype ).
+ append_byte ( command ).
+ append ( payload );
+}
+
Added: code/player/trunk/server/drivers/mixed/nxt/src/nxtdc.hh
===================================================================
--- code/player/trunk/server/drivers/mixed/nxt/src/nxtdc.hh
(rev 0)
+++ code/player/trunk/server/drivers/mixed/nxt/src/nxtdc.hh 2010-12-11
21:51:30 UTC (rev 9005)
@@ -0,0 +1,211 @@
+/*
+ * Player - One Hell of a Robot Server
+ * Copyright (C) 2010
+ * Alejandro R. Mosteo
+ *
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <libusb.h>
+#include <stdexcept>
+#include <vector>
+
+namespace NXT
+ {
+
+ using namespace std;
+
+ typedef vector<unsigned char> protobuffer;
+
+ class buffer : public protobuffer
+ {
+ public:
+ buffer & append_byte ( uint8_t byte );
+ buffer & append_word ( uint16_t word );
+ buffer & append ( const buffer & buf );
+ // return self to chain calls
+
+ void dump ( const string & header ) const; // Debug to stdout
+ };
+
+ class transport
+ {
+ public:
+ virtual void write ( const buffer &buf ) = 0;
+ virtual buffer read ( void ) = 0;
+ };
+
+ class USB_transport : public transport
+ {
+ public:
+ USB_transport ( void );
+ ~USB_transport ( void );
+ virtual void write ( const buffer &buf );
+ virtual buffer read ( void );
+ private:
+ libusb_context *context_;
+ libusb_device_handle *handle_;
+
+ void usb_check ( int usb_error );
+ };
+
+ class nxt_error : public runtime_error
+ {
+ public :
+ nxt_error ( const char *s ) : runtime_error ( s ) {};
+ nxt_error ( const string & s ) : runtime_error ( s ) {};
+ };
+
+ enum motors
+ {
+ A = 0x00,
+ B = 0x01,
+ C = 0x02,
+ All = 0xFF
+ };
+
+ enum motor_modes
+ {
+ motor_on = 0x01,
+ motor_brake = 0x02,
+ motor_regulated = 0x04
+ };
+
+ enum regulation_modes
+ {
+ regulation_motor_idle = 0x00,
+ regulation_motor_speed = 0x01,
+ regulation_motor_sync = 0x02
+ };
+
+ enum motor_run_states
+ {
+ motor_run_state_idle = 0x00,
+ motor_run_state_ramp_up = 0x10,
+ motor_run_state_running = 0x20,
+ motor_run_state_rampdown = 0x40
+ };
+
+ typedef struct
+ {
+ uint8_t protocol_minor;
+ uint8_t protocol_major;
+ uint8_t firmware_minor;
+ uint8_t firmware_major;
+ } versions;
+
+ typedef struct
+ {
+ char brick_name[15]; // Null-terminated
+ char bluetooth_address[7]; // Null-terminated
+ } device_info;
+
+ typedef struct
+ {
+ uint8_t motor;
+ int8_t power_pct;
+ motor_modes mode;
+ regulation_modes regulation;
+ int8_t turn_ratio;
+ motor_run_states state;
+ int32_t tacho_limit; // programmed limit for current
movement, if any
+ int32_t tacho_count; // current tacho count since last
reset (acummulated odometry)
+ int32_t block_tacho_count; // current pos relative to last
programmed pos (delta odometry)
+ int32_t rotation_count; // current pos relative to the last
reset of rot. counter
+ } output_state;
+ // Beware: the delta is since last command, not since last reading!
+
+ class brick
+ {
+
+ public:
+
+ // Connect to the first brick found
+ // TODO: give some means of connecting to a particular brick.
+ // As is, this library serves only for using with a single brick.
+ brick ( void );
+
+ ~brick ( void );
+
+ // Execute a prepared command
+ // When with_feedback, the brick is asked to confirm proper execution
+ // Returns the reply buffer, with the reply flag byte, status, and etc.
+ // or an empty buffer if !with_feedback
+ buffer execute ( const buffer &command, bool with_feedback = false );
+
+ // PREPARED COMMANDS
+ // That you an store and execute with or without feedback
+
+ buffer prepare_play_tone ( uint16_t tone_Hz, uint16_t duration_ms );
+
+ buffer prepare_output_state (
+ motors motor,
+ int8_t power_pct,
+ motor_modes mode = motor_brake,
+ regulation_modes regulation = regulation_motor_speed,
+ int8_t turn_ratio = 0,
+ motor_run_states state = motor_run_state_running,
+ uint32_t tacho_count = 0 );
+ // Full motor control; refer to NXT docs for precise meanings...
+
+ buffer prepare_reset_motor_position ( motors motor, bool
relative_to_last_position = false );
+ buffer prepare_stop_sound_playback ( void );
+ buffer prepare_keep_alive ( void );
+
+ // DIRECT PERFORMING (WITH FEEDBACK)
+ // If you don't want the feedback overhead, use execute with prepared
commands
+ // Errors will be reported as thrown nxt_error
+
+ void play_tone ( uint16_t tone_Hz, uint16_t duration_ms );
+
+ // Simple motor control
+ void set_motor ( motors motor, int8_t power_pct );
+
+ output_state get_motor_state ( motors motor );
+
+ // In millivolts
+ uint16_t get_battery_level ( void );
+
+ versions get_version ( void );
+
+ device_info get_device_info ( void );
+
+ // Run a 10-second loop of play_tone, to get the average time per
commands
+ // For testing purposes, yes.
+ void msg_rate_check ( void );
+
+ private:
+
+ enum telegram_types
+ {
+ direct_command_with_response = 0x00,
+ system_command_with_response = 0x01,
+ reply = 0x02,
+ direct_command_without_response = 0x80,
+ system_command_without_response = 0x80
+ };
+
+ buffer assemble ( telegram_types teltype,
+ uint8_t command,
+ const buffer & payload );
+ // Assembles the full telegram to be sent over usb or bluetooth.
+
+ USB_transport link_;
+ // For now is a fixed USB transport, but we could easily add bluetooth
here
+
+ };
+
+}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Oracle to DB2 Conversion Guide: Learn learn about native support for PL/SQL,
new data types, scalar functions, improved concurrency, built-in packages,
OCI, SQL*Plus, data movement tools, best practices and more.
http://p.sf.net/sfu/oracle-sfdev2dev
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
