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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18  *
19  */
20 
21 #include "canio.h"
22 #include "canio_kvaser.h"
23 #include "usb_packet.h"
24 #include <libplayercore/playercore.h>
25 
26 #ifndef BusType
27 typedef enum { UNKNOWN, RS232, CANBUS, USB } BusType;
28 #endif
29 
30 #define SPEEDTHRESH 0.01
31 
32 // Forward declarations
33 class rmp_frame_t;
34 
35 
36 // Driver for robotic Segway
37 class SegwayRMP : public ThreadedDriver
38 {
39 public:
40  // Constructors etc
41  SegwayRMP(ConfigFile* cf, int section);
42  ~SegwayRMP();
43 
44  // Setup/shutdown routines.
45  virtual int MainSetup();
46  virtual void MainQuit();
47  virtual int ProcessMessage(QueuePointer & resp_queue,
48  player_msghdr * hdr,
49  void * data);
51  int CanBusSetup();
52 
54  int USBSetup();
55 
56 protected:
57 
58  // Supported interfaces
59  // Position2d device
60  player_devaddr_t position_id;
61  player_position2d_data_t position_data;
62 
63  // Position3d device
64  player_devaddr_t position3d_id;
65  player_position3d_data_t position3d_data;
66 
67  // Main Battery device
68  player_devaddr_t power_base_id;
69  player_power_data_t power_base_data;
70 
71  // UI Battery device
72  player_devaddr_t power_ui_id;
73  player_power_data_t power_ui_data;
74 
75 private:
76 
77  BusType bus_type;
78  const char* portname;
79  const char* caniotype;
80  const char* usb_device;
81 
82  int timeout_counter;
83 
84  float max_xspeed, max_yawspeed;
85 
86  bool firstread;
87 
88  DualCANIO *canio;
89  USBIO *usbio;
90 
91  float curr_xspeed, curr_yawspeed;
92  float last_xspeed, last_yawspeed;
93 
94  // Flag to override the default update of 20Hz
95  bool speed_change;
96 
97  // Flag set if motors can be enabled (i.e., enable them to be
98  // enabled). Set by a config request.
99  bool motor_allow_enable;
100 
101  // Flag set if motors are currently enabled
102  bool motor_enabled;
103 
104  // For handling rollover
105  uint32_t last_raw_yaw, last_raw_left, last_raw_right, last_raw_foreaft;
106 
107  // Odometry calculation
108  double odom_x, odom_y, odom_yaw;
109 
110  // helper to handle config requests
111  int HandlePositionConfig(QueuePointer &resp_queue, uint32_t subtype, void* data, size_t len);
112 
113  // helper to handle config requests
114  int HandlePosition3DConfig(QueuePointer &resp_queue, uint32_t subtype, void* data, size_t len);
115 
116  // helper to read a cycle of data from the RMP
117  int Read();
118  int CanBusRead();
119  int USBRead();
120 
121  // Calculate the difference between two raw counter values, taking care
122  // of rollover.
123  int Diff(uint32_t from, uint32_t to, bool first);
124 
125  // helper to write a packet
126  int Write(CanPacket& pkt);
127 
128  // Main function for device thread.
129  virtual void Main();
130 
131  // helper to create a status command packet from the given args
132  void MakeStatusCommand(CanPacket* pkt, uint16_t cmd, uint16_t val);
133 
134  // helper to take a player command and turn it into a CAN command packet
135  void MakeVelocityCommand(CanPacket* pkt, int32_t xspeed, int32_t yawspeed);
136 
137  void MakeShutdownCommand(CanPacket* pkt);
138 
139  void UpdateData(rmp_frame_t *);
140 };
141 
142 

Last updated 12 September 2005 21:38:45