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
30typedef enum { UNKNOWN, RS232, CANBUS, USB } BusType;
31#endif
32
33#define SPEEDTHRESH 0.01
34
35// Forward declarations
36class rmp_frame_t;
37
38
39// Driver for robotic Segway
41{
42public:
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
59protected:
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
78private:
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
Definition canio.h:46
Class for loading configuration file information.
Definition configfile.h:197
Definition canio.h:108
An autopointer for the message queue.
Definition message.h:74
Definition segwayrmp.h:41
void UpdateData(rmp_frame_t *)
Definition segwayrmp.cc:999
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition segwayrmp.cc:589
virtual void Main()
Main method for driver thread.
Definition segwayrmp.cc:421
int USBSetup()
Sets up the USB bus for communication.
Definition segwayrmp.cc:368
int CanBusSetup()
Sets up the CAN bus for communication.
Definition segwayrmp.cc:341
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition segwayrmp.cc:298
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition segwayrmp.cc:388
Base class for drivers which oeprate with a thread.
Definition driver.h:553
Definition usb_packet.h:71
Definition rmp_frame.h:68
A device address.
Definition player.h:146
Generic message header.
Definition player.h:162