segwayrmp.h
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2003 John Sweeney & Brian Gerkey
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program 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 General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include "canio.h"
#include "canio_kvaser.h"
#include "usb_packet.h"
#include <libplayercore/playercore.h>
#ifndef BusType
typedef enum { UNKNOWN, RS232, CANBUS, USB } BusType;
#endif
#define SPEEDTHRESH 0.01
// Forward declarations
class rmp_frame_t;
// Driver for robotic Segway
class SegwayRMP : public ThreadedDriver
{
public:
// Constructors etc
SegwayRMP(ConfigFile* cf, int section);
~SegwayRMP();
// Setup/shutdown routines.
virtual int MainSetup();
virtual void MainQuit();
virtual int ProcessMessage(QueuePointer & resp_queue,
player_msghdr * hdr,
void * data);
int CanBusSetup();
int USBSetup();
protected:
// Supported interfaces
// Position2d device
player_devaddr_t position_id;
player_position2d_data_t position_data;
// Position3d device
player_devaddr_t position3d_id;
player_position3d_data_t position3d_data;
// Main Battery device
player_devaddr_t power_base_id;
player_power_data_t power_base_data;
// UI Battery device
player_devaddr_t power_ui_id;
player_power_data_t power_ui_data;
private:
BusType bus_type;
const char* portname;
const char* caniotype;
const char* usb_device;
int timeout_counter;
float max_xspeed, max_yawspeed;
bool firstread;
DualCANIO *canio;
USBIO *usbio;
float curr_xspeed, curr_yawspeed;
float last_xspeed, last_yawspeed;
// Flag to override the default update of 20Hz
bool speed_change;
// Flag set if motors can be enabled (i.e., enable them to be
// enabled). Set by a config request.
bool motor_allow_enable;
// Flag set if motors are currently enabled
bool motor_enabled;
// For handling rollover
uint32_t last_raw_yaw, last_raw_left, last_raw_right, last_raw_foreaft;
// Odometry calculation
double odom_x, odom_y, odom_yaw;
// helper to handle config requests
int HandlePositionConfig(QueuePointer &resp_queue, uint32_t subtype, void* data, size_t len);
// helper to handle config requests
int HandlePosition3DConfig(QueuePointer &resp_queue, uint32_t subtype, void* data, size_t len);
// helper to read a cycle of data from the RMP
int Read();
int CanBusRead();
int USBRead();
// Calculate the difference between two raw counter values, taking care
// of rollover.
int Diff(uint32_t from, uint32_t to, bool first);
// helper to write a packet
int Write(CanPacket& pkt);
// Main function for device thread.
virtual void Main();
// helper to create a status command packet from the given args
void MakeStatusCommand(CanPacket* pkt, uint16_t cmd, uint16_t val);
// helper to take a player command and turn it into a CAN command packet
void MakeVelocityCommand(CanPacket* pkt, int32_t xspeed, int32_t yawspeed);
void MakeShutdownCommand(CanPacket* pkt);
void UpdateData(rmp_frame_t *);
};
ConfigFile
Class for loading configuration file information.
Definition: configfile.h:196
SegwayRMP::MainSetup
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition: segwayrmp.cc:293
player_msghdr
Generic message header.
Definition: player.h:157
SegwayRMP::ProcessMessage
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: segwayrmp.cc:578
SegwayRMP
Definition: segwayrmp.h:37
CanPacket
Definition: canio.h:45
player_devaddr
A device address.
Definition: player.h:141
rmp_frame_t
Definition: rmp_frame.h:70
ThreadedDriver
Base class for drivers which oeprate with a thread.
Definition: driver.h:544
player_position3d_data
Data: state (PLAYER_POSITION3D_DATA_STATE)
Definition: player_interfaces.h:2470
player_power_data
Data: voltage (PLAYER_POWER_DATA_STATE)
Definition: player_interfaces.h:291
QueuePointer
An autopointer for the message queue.
Definition: message.h:73
SegwayRMP::USBSetup
int USBSetup()
Sets up the USB bus for communication.
Definition: segwayrmp.cc:357
SegwayRMP::UpdateData
void UpdateData(rmp_frame_t *)
Definition: segwayrmp.cc:979
SegwayRMP::Main
virtual void Main()
Main method for driver thread.
Definition: segwayrmp.cc:410
SegwayRMP::MainQuit
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition: segwayrmp.cc:377
USBIO
Definition: usb_packet.h:70
DualCANIO
Definition: canio.h:107
player_position2d_data
position2d data
Definition: player_interfaces.h:606
SegwayRMP::CanBusSetup
int CanBusSetup()
Sets up the CAN bus for communication.
Definition: segwayrmp.cc:336

Last updated 12 September 2005 21:38:45