rmp_frame.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"
#define RMP_CAN_ID_SHUTDOWN 0x0412
#define RMP_CAN_ID_COMMAND 0x0413
#define RMP_CAN_ID_MSG1 0x0400
#define RMP_CAN_ID_MSG2 0x0401
#define RMP_CAN_ID_MSG3 0x0402
#define RMP_CAN_ID_MSG4 0x0403
#define RMP_CAN_ID_MSG5 0x0404
#define RMP_CAN_ID_MSG6 0x0405
#define RMP_CAN_ID_MSG7 0x0406
#define RMP_CAN_ID_MSG8 0x0407
#define RMP_CAN_CMD_NONE 0
#define RMP_CAN_CMD_MAX_VEL 10
#define RMP_CAN_CMD_MAX_ACCL 11
#define RMP_CAN_CMD_MAX_TURN 12
#define RMP_CAN_CMD_GAIN_SCHED 13
#define RMP_CAN_CMD_CURR_LIMIT 14
#define RMP_CAN_CMD_RST_INT 50
#define RMP_CAN_RST_RIGHT 0x01
#define RMP_CAN_RST_LEFT 0x02
#define RMP_CAN_RST_YAW 0x04
#define RMP_CAN_RST_FOREAFT 0x08
#define RMP_CAN_RST_ALL (RMP_CAN_RST_RIGHT | \
RMP_CAN_RST_LEFT | \
RMP_CAN_RST_YAW | \
RMP_CAN_RST_FOREAFT)
#define RMP_COUNT_PER_M 33215
#define RMP_COUNT_PER_DEG 7.8
#define RMP_COUNT_PER_M_PER_S 332
#define RMP_COUNT_PER_DEG_PER_S 7.8
#define RMP_COUNT_PER_MM_PER_S 0.32882963
#define RMP_COUNT_PER_DEG_PER_SS 7.8
#define RMP_COUNT_PER_REV 112644
#define RMP_MAX_TRANS_VEL_MM_S 3576
#define RMP_MAX_ROT_VEL_DEG_S 18 // from rmi_demo: 1300*0.013805056
#define RMP_MAX_TRANS_VEL_COUNT 1176
#define RMP_MAX_ROT_VEL_COUNT 1024
#define RMP_GEOM_WHEEL_SEP 0.54
// this holds all the RMP data it gives us
class rmp_frame_t
{
public:
int16_t pitch;
int16_t pitch_dot;
int16_t roll;
int16_t roll_dot;
uint32_t yaw;
int16_t yaw_dot;
uint32_t left;
int16_t left_dot;
uint32_t right;
int16_t right_dot;
uint32_t foreaft;
uint16_t frames;
int16_t left_torque;
int16_t right_torque;
int16_t op_mode;
int16_t gain_schedule;
uint16_t ui_battery;
uint16_t powerbase_battery;
int16_t rec_speed;
int16_t rec_turn;
uint8_t ready;
rmp_frame_t() : ready(0) {}
// Adds a new packet to this frame
void AddPacket(const CanPacket &pkt);
// Is this frame ready (i.e., did we get all 5 messages)?
bool IsReady() { return ready == 0xFF; }
};
/* Takes a CAN packet from the RMP and parses it into a
* rmp_frame_t struct. sets the ready bitfield
* depending on which CAN packet we have. when
* ready == 0x1F, then we have gotten 5 packets, so everything
* is filled in.
*
* returns:
*/
inline void
rmp_frame_t::AddPacket(const CanPacket &pkt)
{
bool known = true;
switch(pkt.id)
{
case RMP_CAN_ID_MSG1:
//powerbase_battery = pkt.GetSlot(2);
break;
case RMP_CAN_ID_MSG2:
pitch = pkt.GetSlot(0);
pitch_dot = pkt.GetSlot(1);
roll = pkt.GetSlot(2);
roll_dot = pkt.GetSlot(3);
break;
case RMP_CAN_ID_MSG3:
left_dot = (int16_t) pkt.GetSlot(0);
right_dot = (int16_t) pkt.GetSlot(1);
yaw_dot = (int16_t) pkt.GetSlot(2);
frames = pkt.GetSlot(3);
break;
case RMP_CAN_ID_MSG4:
left = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) |
(uint32_t)pkt.GetSlot(0));
right = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) |
(uint32_t)pkt.GetSlot(2));
break;
case RMP_CAN_ID_MSG5:
foreaft = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) |
(uint32_t)pkt.GetSlot(0));
yaw = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) |
(uint32_t)pkt.GetSlot(2));
break;
case RMP_CAN_ID_MSG6:
left_torque = (int16_t) pkt.GetSlot(0);
right_torque = (int16_t) pkt.GetSlot(1);
break;
case RMP_CAN_ID_MSG7:
op_mode = (int16_t) pkt.GetSlot(0);
gain_schedule = (int16_t) pkt.GetSlot(1);
ui_battery = (uint16_t) pkt.GetSlot(2);
powerbase_battery = (uint16_t) pkt.GetSlot(3);
break;
case RMP_CAN_ID_MSG8:
rec_speed = (int16_t) pkt.GetSlot(0);
rec_turn = (int16_t) pkt.GetSlot(1);
break;
default:
known = false;
break;
}
// now set the ready flags
if(known)
ready |= (1 << (pkt.id & 0xFF));
}
CanPacket
Definition: canio.h:45
rmp_frame_t
Definition: rmp_frame.h:70

Last updated 12 September 2005 21:38:45