segwayrmp.h
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2003 John Sweeney & Brian Gerkey
4  *
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
18  *
19  */
20 
21 #include "canio.h"
22 #if defined HAVE_CANLIB
23 #include "canio_kvaser.h"
24 #endif
25 
26 #include "usb_packet.h"
27 #include <libplayercore/playercore.h>
28 
29 #ifndef BusType
30 typedef enum { UNKNOWN, RS232, CANBUS, USB } BusType;
31 #endif
32 
33 #define SPEEDTHRESH 0.01
34 
35 // Forward declarations
36 class rmp_frame_t;
37 
38 
39 // Driver for robotic Segway
40 class SegwayRMP : public ThreadedDriver
41 {
42 public:
43  // Constructors etc
44  SegwayRMP(ConfigFile* cf, int section);
45  ~SegwayRMP();
46 
47  // Setup/shutdown routines.
48  virtual int MainSetup();
49  virtual void MainQuit();
50  virtual int ProcessMessage(QueuePointer & resp_queue,
51  player_msghdr * hdr,
52  void * data);
54  int CanBusSetup();
55 
57  int USBSetup();
58 
59 protected:
60 
61  // Supported interfaces
62  // Position2d device
63  player_devaddr_t position_id;
64  player_position2d_data_t position_data;
65 
66  // Position3d device
67  player_devaddr_t position3d_id;
68  player_position3d_data_t position3d_data;
69 
70  // Main Battery device
71  player_devaddr_t power_base_id;
72  player_power_data_t power_base_data;
73 
74  // UI Battery device
75  player_devaddr_t power_ui_id;
76  player_power_data_t power_ui_data;
77 
78 private:
79 
80  BusType bus_type;
81  const char* portname;
82  const char* caniotype;
83  const char* usb_device;
84 
85  int timeout_counter;
86 
87  float max_xspeed, max_yawspeed;
88 
89  bool firstread;
90 
91  DualCANIO *canio;
92  USBIO *usbio;
93 
94  float curr_xspeed, curr_yawspeed;
95  float last_xspeed, last_yawspeed;
96 
97  // Flag to override the default update of 20Hz
98  bool speed_change;
99 
100  // Flag set if motors can be enabled (i.e., enable them to be
101  // enabled). Set by a config request.
102  bool motor_allow_enable;
103 
104  // Flag set if motors are currently enabled
105  bool motor_enabled;
106 
107  // For handling rollover
108  uint32_t last_raw_yaw, last_raw_left, last_raw_right, last_raw_foreaft;
109 
110  // Odometry calculation
111  double odom_x, odom_y, odom_yaw;
112 
113  // helper to handle config requests
114  int HandlePositionConfig(QueuePointer &resp_queue, uint32_t subtype, void* data, size_t len);
115 
116  // helper to handle config requests
117  int HandlePosition3DConfig(QueuePointer &resp_queue, uint32_t subtype, void* data, size_t len);
118 
119  // helper to read a cycle of data from the RMP
120  int Read();
121  int CanBusRead();
122  int USBRead();
123 
124  // Calculate the difference between two raw counter values, taking care
125  // of rollover.
126  int Diff(uint32_t from, uint32_t to, bool first);
127 
128  // helper to write a packet
129  int Write(CanPacket& pkt);
130 
131  // Main function for device thread.
132  virtual void Main();
133 
134  // helper to create a status command packet from the given args
135  void MakeStatusCommand(CanPacket* pkt, uint16_t cmd, uint16_t val);
136 
137  // helper to take a player command and turn it into a CAN command packet
138  void MakeVelocityCommand(CanPacket* pkt, int32_t xspeed, int32_t yawspeed);
139 
140  void MakeShutdownCommand(CanPacket* pkt);
141 
142  void UpdateData(rmp_frame_t *);
143 };
144 
145 
virtual void Main()
Main method for driver thread.
Definition: segwayrmp.cc:421
virtual void Publish(player_devaddr_t addr, QueuePointer &queue, uint8_t type, uint8_t subtype, void *src=NULL, size_t deprecated=0, double *timestamp=NULL, bool copy=true)
Publish a message via one of this driver's interfaces.
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition: segwayrmp.cc:388
Definition: segwayrmp.h:41
Definition: usb_packet.h:71
Definition: canio.h:46
Generic message header.
Definition: player.h:162
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:166
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
double ReadAngle(int section, const char *name, double value)
Read an angle (includes unit conversion).
double ReadLength(int section, const char *name, double value)
Read a length (includes unit conversion, if any).
void ProcessMessages(void)
Process pending messages.
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
void UpdateData(rmp_frame_t *)
Definition: segwayrmp.cc:999
#define PLAYER_ERROR2(msg, a, b)
Error message macros.
Definition: error.h:83
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: segwayrmp.cc:589
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
#define PLAYER_WARN2(msg, a, b)
Error message macros.
Definition: error.h:91
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
#define PLAYER_MSGTYPE_RESP_NACK
A negative response message.
Definition: player.h:125
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
Class for loading configuration file information.
Definition: configfile.h:197
#define PLAYER_CAPABILITIES_REQ
Capability request message type.
Definition: player.h:397
A device address.
Definition: player.h:146
An autopointer for the message queue.
Definition: message.h:74
Definition: canio_kvaser.h:28
#define PLAYER_ERROR1(msg, a)
Error message macros.
Definition: error.h:82
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
Base class for drivers which oeprate with a thread.
Definition: driver.h:553
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
Definition: rmp_frame.h:68
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
Definition: canio.h:108
Base class for all drivers.
Definition: driver.h:109
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition: segwayrmp.cc:298
#define PLAYER_MSG0(level, msg)
General messages.
Definition: error.h:105
int CanBusSetup()
Sets up the CAN bus for communication.
Definition: segwayrmp.cc:341
#define PLAYER_MSG2(level, msg, a, b)
Error message macros.
Definition: error.h:107
int USBSetup()
Sets up the USB bus for communication.
Definition: segwayrmp.cc:368
uint16_t index
Which device of that interface.
Definition: player.h:155
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:164
uint16_t interf
The interface provided by the device; must be one of PLAYER_*_CODE.
Definition: player.h:153
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76
T max(T a, T b)
Return the maximum of a, b.
Definition: utility.h:126