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 <libplayercore/playercore.h>
00024 
00025 
00026 // Forward declarations
00027 class rmp_frame_t;
00028 
00029 
00030 // Driver for robotic Segway
00031 class SegwayRMP : public Driver
00032 {
00033   public: 
00034     // Constructors etc
00035     SegwayRMP(ConfigFile* cf, int section);
00036     ~SegwayRMP();
00037 
00038     // Setup/shutdown routines.
00039     virtual int Setup();
00040     virtual int Shutdown();
00041     virtual int ProcessMessage(QueuePointer & resp_queue,
00042                                player_msghdr * hdr,
00043                                void * data);
00044 
00045   protected:
00046 
00047     // Supported interfaces
00048     player_devaddr_t position_id;
00049     player_position2d_data_t position_data;
00050 
00051     player_devaddr_t position3d_id;
00052     player_position3d_data_t position3d_data;
00053   
00054     player_devaddr_t power_id;
00055     player_power_data_t power_data;
00056 
00057   private:
00058 
00059     const char* portname;
00060     const char* caniotype;
00061 
00062     int timeout_counter;
00063 
00064     float max_xspeed, max_yawspeed;
00065 
00066     bool firstread;
00067 
00068     DualCANIO *canio;
00069 
00070     float curr_xspeed, curr_yawspeed;
00071 
00072     // Flag set if motors can be enabled (i.e., enable them to be
00073     // enabled).  Set by a config request.
00074     bool motor_allow_enable;
00075 
00076     // Flag set if motors are currently enabled
00077     bool motor_enabled;
00078 
00079     // For handling rollover
00080     uint32_t last_raw_yaw, last_raw_left, last_raw_right, last_raw_foreaft;
00081 
00082     // Odometry calculation
00083     double odom_x, odom_y, odom_yaw;
00084 
00085     // helper to handle config requests
00086     int HandlePositionConfig(QueuePointer &resp_queue, uint32_t subtype, void* data, size_t len);
00087 
00088     // helper to handle config requests
00089     int HandlePosition3DConfig(QueuePointer &resp_queue, uint32_t subtype, void* data, size_t len);
00090 
00091     // helper to read a cycle of data from the RMP
00092     int Read();
00093     
00094     // Calculate the difference between two raw counter values, taking care
00095     // of rollover.
00096     int Diff(uint32_t from, uint32_t to, bool first);
00097 
00098     // helper to write a packet
00099     int Write(CanPacket& pkt);
00100 
00101     // Main function for device thread.
00102     virtual void Main();
00103 
00104     // helper to create a status command packet from the given args
00105     void MakeStatusCommand(CanPacket* pkt, uint16_t cmd, uint16_t val);
00106 
00107     // helper to take a player command and turn it into a CAN command packet
00108     void MakeVelocityCommand(CanPacket* pkt, int32_t xspeed, int32_t yawspeed);
00109     
00110     void MakeShutdownCommand(CanPacket* pkt);
00111 
00112     void UpdateData(rmp_frame_t *);
00113 };
00114 
00115
Last updated 12 September 2005 21:38:45