nxtdc.hh
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2010
4  * Alejandro R. Mosteo
5  *
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public
9  * License as published by the Free Software Foundation; either
10  * version 2.1 of the License, or (at your option) any later version.
11  *
12  * This library is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15  * Lesser General Public License for more details.
16  *
17  * You should have received a copy of the GNU Lesser General Public
18  * License along with this library; if not, write to the Free Software
19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20  */
21 
22 #include <libusb.h>
23 #include <stdexcept>
24 #include <vector>
25 
26 namespace NXT
27  {
28 
29  using namespace std;
30 
31  typedef vector<unsigned char> protobuffer;
32 
33  class buffer : public protobuffer
34  {
35  public:
36  buffer & append_byte ( uint8_t byte );
37  buffer & append_word ( uint16_t word );
38  buffer & append ( const buffer & buf );
39  // return self to chain calls
40 
41  void dump ( const string & header ) const; // Debug to stdout
42  };
43 
44  class transport
45  {
46  public:
47  virtual void write ( const buffer &buf ) = 0;
48  virtual buffer read ( void ) = 0;
49  };
50 
51  class USB_transport : public transport
52  {
53  public:
54  USB_transport ( void );
55  ~USB_transport ( void );
56  virtual void write ( const buffer &buf );
57  virtual buffer read ( void );
58  private:
59  libusb_context *context_;
60  libusb_device_handle *handle_;
61 
62  void usb_check ( int usb_error );
63  };
64 
65  class nxt_error : public runtime_error
66  {
67  public :
68  nxt_error ( const char *s ) : runtime_error ( s ) {};
69  nxt_error ( const string & s ) : runtime_error ( s ) {};
70  };
71 
72  enum motors
73  {
74  A = 0x00,
75  B = 0x01,
76  C = 0x02,
77  All = 0xFF
78  };
79 
80  enum motor_modes
81  {
82  motor_on = 0x01,
83  motor_brake = 0x02,
84  motor_regulated = 0x04
85  };
86 
87  enum regulation_modes
88  {
89  regulation_motor_idle = 0x00,
90  regulation_motor_speed = 0x01,
91  regulation_motor_sync = 0x02
92  };
93 
94  enum motor_run_states
95  {
96  motor_run_state_idle = 0x00,
97  motor_run_state_ramp_up = 0x10,
98  motor_run_state_running = 0x20,
99  motor_run_state_rampdown = 0x40
100  };
101 
102  typedef struct
103  {
104  uint8_t protocol_minor;
105  uint8_t protocol_major;
106  uint8_t firmware_minor;
107  uint8_t firmware_major;
108  } versions;
109 
110  typedef struct
111  {
112  char brick_name[15]; // Null-terminated
113  char bluetooth_address[7]; // Null-terminated
114  } device_info;
115 
116  typedef struct
117  {
118  uint8_t motor;
119  int8_t power_pct;
120  motor_modes mode;
121  regulation_modes regulation;
122  int8_t turn_ratio;
123  motor_run_states state;
124  int32_t tacho_limit; // programmed limit for current movement, if any
125  int32_t tacho_count; // current tacho count since last reset (acummulated odometry)
126  int32_t block_tacho_count; // current pos relative to last programmed pos (delta odometry)
127  int32_t rotation_count; // current pos relative to the last reset of rot. counter
128  } output_state;
129  // Beware: the delta is since last command, not since last reading!
130 
131  class brick
132  {
133 
134  public:
135 
136  // Connect to the first brick found
137  // TODO: give some means of connecting to a particular brick.
138  // As is, this library serves only for using with a single brick.
139  brick ( void );
140 
141  ~brick ( void );
142 
143  // Execute a prepared command
144  // When with_feedback, the brick is asked to confirm proper execution
145  // Returns the reply buffer, with the reply flag byte, status, and etc.
146  // or an empty buffer if !with_feedback
147  buffer execute ( const buffer &command, bool with_feedback = false );
148 
149  // PREPARED COMMANDS
150  // That you an store and execute with or without feedback
151 
152  buffer prepare_play_tone ( uint16_t tone_Hz, uint16_t duration_ms );
153 
154  buffer prepare_output_state (
155  motors motor,
156  int8_t power_pct,
157  motor_modes mode = motor_brake,
158  regulation_modes regulation = regulation_motor_speed,
159  int8_t turn_ratio = 0,
160  motor_run_states state = motor_run_state_running,
161  uint32_t tacho_count = 0 );
162  // Full motor control; refer to NXT docs for precise meanings...
163 
164  buffer prepare_reset_motor_position ( motors motor, bool relative_to_last_position = false );
165  buffer prepare_stop_sound_playback ( void );
166  buffer prepare_keep_alive ( void );
167 
168  // DIRECT PERFORMING (WITH FEEDBACK)
169  // If you don't want the feedback overhead, use execute with prepared commands
170  // Errors will be reported as thrown nxt_error
171 
172  void play_tone ( uint16_t tone_Hz, uint16_t duration_ms );
173 
174  // Simple motor control
175  void set_motor ( motors motor, int8_t power_pct );
176 
177  output_state get_motor_state ( motors motor );
178 
179  // In millivolts
180  uint16_t get_battery_level ( void );
181 
182  versions get_version ( void );
183 
184  device_info get_device_info ( void );
185 
186  // Run a 10-second loop of play_tone, to get the average time per commands
187  // For testing purposes, yes.
188  void msg_rate_check ( void );
189 
190  private:
191 
192  enum telegram_types
193  {
194  direct_command_with_response = 0x00,
195  system_command_with_response = 0x01,
196  reply = 0x02,
197  direct_command_without_response = 0x80,
198  system_command_without_response = 0x80
199  };
200 
201  buffer assemble ( telegram_types teltype,
202  uint8_t command,
203  const buffer & payload );
204  // Assembles the full telegram to be sent over usb or bluetooth.
205 
206  USB_transport link_;
207  // For now is a fixed USB transport, but we could easily add bluetooth here
208 
209  };
210 
211 }
#define PLAYER_POSITION1D_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:3485
#define PLAYER_BUMPER_REQ_GET_GEOM
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1916
Definition: nxtdc.hh:65
position 2d velocity command
Definition: player_interfaces.h:617
uint32_t bumpers_count
the number of valid bumper readings
Definition: player_interfaces.h:1925
double proll
roll [rad]
Definition: player.h:236
#define PLAYER_MSG1(level, msg, a)
Error message macros.
Definition: error.h:105
#define PLAYER_MSG3(level, msg, a, b, c)
Error message macros.
Definition: error.h:107
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:620
Command: state (PLAYER_POSITION1D_CMD_VEL)
Definition: player_interfaces.h:3570
uint32_t host
The "host" on which the device resides.
Definition: player.h:147
#define PLAYER_IR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:2108
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:158
#define PLAYER_POSITION1D_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:3512
#define PLAYER_POSITION1D_REQ_SPEED_PID
Request/reply subtype: set speed PID params.
Definition: player_interfaces.h:3503
uint32_t poses_count
The number of valid poses.
Definition: player_interfaces.h:788
player_bumper_define_t * bumper_def
geometry of each bumper
Definition: player_interfaces.h:1953
Data: state (PLAYER_POSITION1D_DATA_STATE)
Definition: player_interfaces.h:3542
Definition: nxtdc.hh:33
double px
X [m].
Definition: player.h:230
Data: ranges (PLAYER_SONAR_DATA_RANGES)
Definition: player_interfaces.h:771
Data AND Request/reply: geometry.
Definition: player_interfaces.h:785
uint32_t poses_count
the number of ir samples returned by this robot
Definition: player_interfaces.h:2133
Definition: nxtdc.hh:102
Data AND Request/reply: bumper geometry.
Definition: player_interfaces.h:1948
Generic message header.
Definition: player.h:160
double pz
Z [m].
Definition: player.h:234
#define PLAYER_POSITION1D_REQ_POSITION_PID
Request/reply subtype: set position PID params.
Definition: player_interfaces.h:3506
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:657
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:663
#define PLAYER_POSITION1D_CMD_VEL
Command subtype: velocity command.
Definition: player_interfaces.h:3518
#define PLAYER_IR_REQ_POSE
Request/reply subtype: get pose.
Definition: player_interfaces.h:2102
Request/reply: Query geometry.
Definition: player_interfaces.h:3597
float vel
velocity [m/s] or [rad/s]
Definition: player_interfaces.h:3573
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:165
#define PLAYER_WARN3(msg, a, b, c)
Error message macros.
Definition: error.h:91
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
double px
X [m].
Definition: player.h:219
Request/reply: get pose.
Definition: player_interfaces.h:2130
#define PLAYER_POWER_DATA_STATE
Data subtype: voltage.
Definition: player_interfaces.h:274
#define PLAYER_POWER_MASK_VOLTS
bit masks for the player_power_data_t mask field
Definition: player_interfaces.h:282
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
double pyaw
yaw [rad]
Definition: player.h:240
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:167
uint32_t robot
The "robot" or device collection in which the device resides.
Definition: player.h:150
double ReadTupleFloat(int section, const char *name, int index, double value)
Read a float (double) from a tuple field.
#define PLAYER_POSITION1D_REQ_RESET_ODOM
Request/reply subtype: reset odometry.
Definition: player_interfaces.h:3500
virtual void Main(void)=0
Main method for driver thread.
#define PLAYER_POSITION1D_REQ_POSITION_MODE
Request/reply subtype: position mode.
Definition: player_interfaces.h:3494
int ReadInt(int section, const char *name, int value)
Read an integer value.
player_pose3d_t * poses
the pose of each IR detector on this robot
Definition: player_interfaces.h:2135
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:94
double py
Y [m].
Definition: player.h:221
float radius
radius of curvature [m] - zero for straight lines
Definition: player_interfaces.h:1938
player_bbox3d_t size
Dimensions of the base (m, m, m).
Definition: player_interfaces.h:3602
player_pose3d_t pose
Pose of the robot base, in the robot cs (m, m, m, rad, rad, rad).
Definition: player_interfaces.h:3600
uint32_t bumper_def_count
The number of valid bumper definitions.
Definition: player_interfaces.h:1951
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:111
player_pose3d_t pose
Pose of the robot base, in the robot cs (m, rad).
Definition: player_interfaces.h:658
#define PLAYER_WARN2(msg, a, b)
Error message macros.
Definition: error.h:90
float * ranges
ranges [m]
Definition: player_interfaces.h:2124
#define PLAYER_POWER_REQ_SET_CHARGING_POLICY
Request subtype: set charging policy.
Definition: player_interfaces.h:277
float pos
(x) [m] or [rad]
Definition: player_interfaces.h:3665
#define PLAYER_POSITION2D_REQ_GET_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:483
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1922
double ppitch
pitch [rad]
Definition: player.h:238
#define PLAYER_POSITION1D_REQ_MOTOR_POWER
Request/reply subtype: motor power.
Definition: player_interfaces.h:3488
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:105
#define PLAYER_SONAR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:761
double sl
Length [m].
Definition: player.h:258
Data: voltage (PLAYER_POWER_DATA_STATE)
Definition: player_interfaces.h:291
Definition: nxtdc.hh:116
float * ranges
The range readings [m].
Definition: player_interfaces.h:776
Request/reply: Set linear speed profile parameters.
Definition: player_interfaces.h:3703
#define PLAYER_POSITION1D_STATUS_ENABLED
Status byte: enabled.
Definition: player_interfaces.h:3536
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
Request/reply: Set odometry.
Definition: player_interfaces.h:3662
#define PLAYER_POSITION1D_REQ_SET_ODOM
Request/reply subtype: set odometry.
Definition: player_interfaces.h:3497
#define PLAYER_BUMPER_DATA_STATE
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1910
#define PLAYER_SONAR_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:755
Definition: nxtdc.hh:131
Class for loading configuration file information.
Definition: configfile.h:195
double sw
Width [m].
Definition: player.h:256
A device address.
Definition: player.h:144
float pos
position [m] or [rad] depending on actuator type
Definition: player_interfaces.h:3545
An autopointer for the message queue.
Definition: message.h:72
double py
Y [m].
Definition: player.h:232
Definition: nxtdc.hh:44
uint8_t * bumpers
array of bumper values
Definition: player_interfaces.h:1927
Definition: nxtdc.hh:51
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:611
#define PLAYER_POSITION1D_REQ_SPEED_PROF
Request/reply subtype: set speed profile params.
Definition: player_interfaces.h:3509
position2d data
Definition: player_interfaces.h:606
position2d geom
Definition: player_interfaces.h:655
uint32_t seq
For keeping track of associated messages.
Definition: player.h:171
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:80
Base class for drivers which oeprate with a thread.
Definition: driver.h:551
uint32_t ranges_count
The number of valid range readings.
Definition: player_interfaces.h:774
uint32_t ranges_count
number of samples
Definition: player_interfaces.h:2122
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
player_pose3d_t * poses
Pose of each sonar, in robot cs.
Definition: player_interfaces.h:790
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:609
Data: ranges (PLAYER_IR_DATA_RANGES)
Definition: player_interfaces.h:2115
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:88
float length
length of the sensor [m]
Definition: player_interfaces.h:1936
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:98
double pa
yaw [rad]
Definition: player.h:223
Base class for all drivers.
Definition: driver.h:107
#define PLAYER_WARN4(msg, a, b, c, d)
Error message macros.
Definition: error.h:92
Definition: nxtdc.hh:110
float speed
max speed [m/s] or [rad/s]
Definition: player_interfaces.h:3706
player_bbox3d_t size
Dimensions of the base (m).
Definition: player_interfaces.h:660
#define PLAYER_POSITION1D_CMD_POS
Command subtype: position command.
Definition: player_interfaces.h:3521
set odometry
Definition: player_interfaces.h:686
Definition: mixed/nxt/src/chronos.hh:28
uint16_t index
Which device of that interface.
Definition: player.h:154
float vel
translational velocities [m/s] or [rad/s] depending on actuator type
Definition: player_interfaces.h:3547
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:163
uint16_t interf
The interface provided by the device; must be one of PLAYER_*_CODE.
Definition: player.h:152
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:75
#define PLAYER_POSITION2D_REQ_SET_ODOM
Request/reply: Set odometry.
Definition: player_interfaces.h:535
player_pose3d_t pose
the local pose of a single bumper
Definition: player_interfaces.h:1934