segwayrmp.h
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2003  John Sweeney & Brian Gerkey
00004  *
00005  *  This program is free software; you can redistribute it and/or modify
00006  *  it under the terms of the GNU General Public License as published by
00007  *  the Free Software Foundation; either version 2 of the License, or
00008  *  (at your option) any later version.
00009  *
00010  *  This program is distributed in the hope that it will be useful,
00011  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  *  GNU General Public License for more details.
00014  *
00015  *  You should have received a copy of the GNU General Public License
00016  *  along with this program; if not, write to the Free Software
00017  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018  *
00019  */
00020 
00021 #include "canio.h"
00022 #include "canio_kvaser.h"
00023 #include "usb_packet.h"
00024 #include <libplayercore/playercore.h>
00025 
00026 #ifndef BusType
00027 typedef enum { UNKNOWN, RS232, CANBUS, USB } BusType;
00028 #endif
00029 
00030 #define SPEEDTHRESH 0.01
00031 
00032 // Forward declarations
00033 class rmp_frame_t;
00034 
00035 
00036 // Driver for robotic Segway
00037 class SegwayRMP : public ThreadedDriver
00038 {
00039 public:
00040         // Constructors etc
00041         SegwayRMP(ConfigFile* cf, int section);
00042         ~SegwayRMP();
00043 
00044         // Setup/shutdown routines.
00045         virtual int MainSetup();
00046         virtual void MainQuit();
00047         virtual int ProcessMessage(QueuePointer & resp_queue,
00048                         player_msghdr * hdr,
00049                         void * data);
00051         int CanBusSetup();
00052 
00054         int USBSetup();
00055 
00056 protected:
00057 
00058         // Supported interfaces
00059         // Position2d device
00060         player_devaddr_t position_id;
00061         player_position2d_data_t position_data;
00062 
00063         // Position3d device
00064         player_devaddr_t position3d_id;
00065         player_position3d_data_t position3d_data;
00066 
00067         // Main Battery device
00068         player_devaddr_t power_base_id;
00069         player_power_data_t power_base_data;
00070 
00071         // UI Battery device
00072         player_devaddr_t power_ui_id;
00073         player_power_data_t power_ui_data;
00074 
00075 private:
00076 
00077         BusType bus_type;
00078         const char* portname;
00079         const char* caniotype;
00080         const char* usb_device;
00081 
00082         int timeout_counter;
00083 
00084         float max_xspeed, max_yawspeed;
00085 
00086         bool firstread;
00087 
00088         DualCANIO *canio;
00089         USBIO *usbio;
00090 
00091         float curr_xspeed, curr_yawspeed;
00092         float last_xspeed, last_yawspeed;
00093 
00094         // Flag to override the default update of 20Hz
00095         bool speed_change;
00096 
00097         // Flag set if motors can be enabled (i.e., enable them to be
00098         // enabled).  Set by a config request.
00099         bool motor_allow_enable;
00100 
00101         // Flag set if motors are currently enabled
00102         bool motor_enabled;
00103 
00104         // For handling rollover
00105         uint32_t last_raw_yaw, last_raw_left, last_raw_right, last_raw_foreaft;
00106 
00107         // Odometry calculation
00108         double odom_x, odom_y, odom_yaw;
00109 
00110         // helper to handle config requests
00111         int HandlePositionConfig(QueuePointer &resp_queue, uint32_t subtype, void* data, size_t len);
00112 
00113         // helper to handle config requests
00114         int HandlePosition3DConfig(QueuePointer &resp_queue, uint32_t subtype, void* data, size_t len);
00115 
00116         // helper to read a cycle of data from the RMP
00117         int Read();
00118         int CanBusRead();
00119         int USBRead();
00120 
00121         // Calculate the difference between two raw counter values, taking care
00122         // of rollover.
00123         int Diff(uint32_t from, uint32_t to, bool first);
00124 
00125         // helper to write a packet
00126         int Write(CanPacket& pkt);
00127 
00128         // Main function for device thread.
00129         virtual void Main();
00130 
00131         // helper to create a status command packet from the given args
00132         void MakeStatusCommand(CanPacket* pkt, uint16_t cmd, uint16_t val);
00133 
00134         // helper to take a player command and turn it into a CAN command packet
00135         void MakeVelocityCommand(CanPacket* pkt, int32_t xspeed, int32_t yawspeed);
00136 
00137         void MakeShutdownCommand(CanPacket* pkt);
00138 
00139         void UpdateData(rmp_frame_t *);
00140 };
00141 
00142