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));
}