rmp_frame.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 
00022 #include "canio.h"
00023 #include "canio_kvaser.h"
00024 
00025 
00026 #define RMP_CAN_ID_SHUTDOWN     0x0412
00027 #define RMP_CAN_ID_COMMAND      0x0413
00028 #define RMP_CAN_ID_MSG1         0x0400
00029 #define RMP_CAN_ID_MSG2         0x0401
00030 #define RMP_CAN_ID_MSG3         0x0402
00031 #define RMP_CAN_ID_MSG4         0x0403
00032 #define RMP_CAN_ID_MSG5         0x0404
00033 
00034 #define RMP_CAN_CMD_NONE                0
00035 #define RMP_CAN_CMD_MAX_VEL             10
00036 #define RMP_CAN_CMD_MAX_ACCL            11
00037 #define RMP_CAN_CMD_MAX_TURN            12
00038 #define RMP_CAN_CMD_GAIN_SCHED          13
00039 #define RMP_CAN_CMD_CURR_LIMIT          14
00040 #define RMP_CAN_CMD_RST_INT             50
00041 
00042 #define RMP_CAN_RST_RIGHT               0x01
00043 #define RMP_CAN_RST_LEFT                0x02
00044 #define RMP_CAN_RST_YAW                 0x04
00045 #define RMP_CAN_RST_FOREAFT             0x08
00046 #define RMP_CAN_RST_ALL                 (RMP_CAN_RST_RIGHT | \
00047                                          RMP_CAN_RST_LEFT | \
00048                                          RMP_CAN_RST_YAW | \
00049                                          RMP_CAN_RST_FOREAFT)
00050 
00051 #define RMP_COUNT_PER_M                 33215
00052 #define RMP_COUNT_PER_DEG               7.8
00053 #define RMP_COUNT_PER_M_PER_S           332
00054 #define RMP_COUNT_PER_DEG_PER_S         7.8
00055 #define RMP_COUNT_PER_MM_PER_S          0.32882963
00056 #define RMP_COUNT_PER_DEG_PER_SS        7.8
00057 #define RMP_COUNT_PER_REV               112644
00058 
00059 #define RMP_MAX_TRANS_VEL_MM_S          3576
00060 #define RMP_MAX_ROT_VEL_DEG_S           18      // from rmi_demo: 1300*0.013805056
00061 #define RMP_MAX_TRANS_VEL_COUNT         1176
00062 #define RMP_MAX_ROT_VEL_COUNT           1024
00063 
00064 #define RMP_GEOM_WHEEL_SEP 0.54
00065 
00066 // this holds all the RMP data it gives us
00067 class rmp_frame_t
00068 {
00069   public:
00070     int16_t pitch;
00071     int16_t pitch_dot;
00072     int16_t roll;
00073     int16_t roll_dot;
00074     uint32_t yaw;
00075     int16_t yaw_dot;
00076     uint32_t left;
00077     int16_t left_dot;
00078     uint32_t right;
00079     int16_t right_dot;
00080     uint32_t foreaft;
00081 
00082     uint16_t frames;
00083     uint16_t battery;
00084     uint8_t  ready;
00085 
00086     rmp_frame_t() : ready(0) {}
00087 
00088     // Adds a new packet to this frame
00089     void AddPacket(const CanPacket &pkt);
00090 
00091     // Is this frame ready (i.e., did we get all 5 messages)?
00092     bool IsReady() { return ready == 0x1F; }
00093 
00094 };
00095 
00096 
00097 
00098 /* Takes a CAN packet from the RMP and parses it into a
00099  * rmp_frame_t struct.  sets the ready bitfield 
00100  * depending on which CAN packet we have.  when
00101  * ready == 0x1F, then we have gotten 5 packets, so everything
00102  * is filled in.
00103  *
00104  * returns: 
00105  */
00106 inline void
00107 rmp_frame_t::AddPacket(const CanPacket &pkt)
00108 {
00109   bool known = true;
00110 
00111   switch(pkt.id) 
00112   {
00113     case RMP_CAN_ID_MSG1:
00114       battery = pkt.GetSlot(2);
00115       break;
00116 
00117     case RMP_CAN_ID_MSG2:
00118       pitch = pkt.GetSlot(0);
00119       pitch_dot = pkt.GetSlot(1);
00120       roll =  pkt.GetSlot(2);
00121       roll_dot =  pkt.GetSlot(3);
00122       break;
00123 
00124     case RMP_CAN_ID_MSG3:
00125       left_dot = (int16_t) pkt.GetSlot(0);
00126       right_dot = (int16_t) pkt.GetSlot(1);
00127       yaw_dot = (int16_t) pkt.GetSlot(2);
00128       frames = pkt.GetSlot(3);
00129       break;
00130 
00131     case RMP_CAN_ID_MSG4:
00132       left = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) | 
00133                         (uint32_t)pkt.GetSlot(0));
00134       right = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) | 
00135                          (uint32_t)pkt.GetSlot(2));
00136       break;
00137 
00138     case RMP_CAN_ID_MSG5:
00139       foreaft = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) | 
00140                            (uint32_t)pkt.GetSlot(0));
00141       yaw = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) | 
00142                        (uint32_t)pkt.GetSlot(2));
00143       break;
00144     default:
00145       known = false;
00146       break;
00147   }
00148 
00149   // now set the ready flags
00150   if(known) 
00151     ready |= (1 << (pkt.id & 0xF));
00152 }
Last updated 12 September 2005 21:38:45