rmp_frame.h
1/*
2 * Player - One Hell of a Robot Server
3 * Copyright (C) 2003 John Sweeney & Brian Gerkey
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
18 *
19 */
20
21#include "canio.h"
22
23#define RMP_CAN_ID_SHUTDOWN 0x0412
24#define RMP_CAN_ID_COMMAND 0x0413
25#define RMP_CAN_ID_MSG1 0x0400
26#define RMP_CAN_ID_MSG2 0x0401
27#define RMP_CAN_ID_MSG3 0x0402
28#define RMP_CAN_ID_MSG4 0x0403
29#define RMP_CAN_ID_MSG5 0x0404
30#define RMP_CAN_ID_MSG6 0x0405
31#define RMP_CAN_ID_MSG7 0x0406
32#define RMP_CAN_ID_MSG8 0x0407
33
34#define RMP_CAN_CMD_NONE 0
35#define RMP_CAN_CMD_MAX_VEL 10
36#define RMP_CAN_CMD_MAX_ACCL 11
37#define RMP_CAN_CMD_MAX_TURN 12
38#define RMP_CAN_CMD_GAIN_SCHED 13
39#define RMP_CAN_CMD_CURR_LIMIT 14
40#define RMP_CAN_CMD_RST_INT 50
41
42#define RMP_CAN_RST_RIGHT 0x01
43#define RMP_CAN_RST_LEFT 0x02
44#define RMP_CAN_RST_YAW 0x04
45#define RMP_CAN_RST_FOREAFT 0x08
46#define RMP_CAN_RST_ALL (RMP_CAN_RST_RIGHT | \
47 RMP_CAN_RST_LEFT | \
48 RMP_CAN_RST_YAW | \
49 RMP_CAN_RST_FOREAFT)
50
51#define RMP_COUNT_PER_M 33215
52#define RMP_COUNT_PER_DEG 7.8
53#define RMP_COUNT_PER_M_PER_S 332
54#define RMP_COUNT_PER_DEG_PER_S 7.8
55#define RMP_COUNT_PER_MM_PER_S 0.32882963
56#define RMP_COUNT_PER_DEG_PER_SS 7.8
57#define RMP_COUNT_PER_REV 112644
58
59#define RMP_MAX_TRANS_VEL_MM_S 3576
60#define RMP_MAX_ROT_VEL_DEG_S 18 // from rmi_demo: 1300*0.013805056
61#define RMP_MAX_TRANS_VEL_COUNT 1176
62#define RMP_MAX_ROT_VEL_COUNT 1024
63
64#define RMP_GEOM_WHEEL_SEP 0.54
65
66// this holds all the RMP data it gives us
68{
69public:
70 int16_t pitch;
71 int16_t pitch_dot;
72 int16_t roll;
73 int16_t roll_dot;
74 uint32_t yaw;
75 int16_t yaw_dot;
76 uint32_t left;
77 int16_t left_dot;
78 uint32_t right;
79 int16_t right_dot;
80 uint32_t foreaft;
81
82 uint16_t frames;
83 int16_t left_torque;
84 int16_t right_torque;
85
86 int16_t op_mode;
87 int16_t gain_schedule;
88 uint16_t ui_battery;
89 uint16_t powerbase_battery;
90
91 int16_t rec_speed;
92 int16_t rec_turn;
93
94 uint8_t ready;
95
96 rmp_frame_t() : ready(0) {}
97
98 // Adds a new packet to this frame
99 void AddPacket(const CanPacket &pkt);
100
101 // Is this frame ready (i.e., did we get all 5 messages)?
102 bool IsReady() { return ready == 0xFF; }
103
104};
105
106
107
108/* Takes a CAN packet from the RMP and parses it into a
109 * rmp_frame_t struct. sets the ready bitfield
110 * depending on which CAN packet we have. when
111 * ready == 0x1F, then we have gotten 5 packets, so everything
112 * is filled in.
113 *
114 * returns:
115 */
116inline void
117rmp_frame_t::AddPacket(const CanPacket &pkt)
118{
119 bool known = true;
120
121 switch(pkt.id)
122 {
123 case RMP_CAN_ID_MSG1:
124 //powerbase_battery = pkt.GetSlot(2);
125 break;
126
127 case RMP_CAN_ID_MSG2:
128 pitch = pkt.GetSlot(0);
129 pitch_dot = pkt.GetSlot(1);
130 roll = pkt.GetSlot(2);
131 roll_dot = pkt.GetSlot(3);
132 break;
133
134 case RMP_CAN_ID_MSG3:
135 left_dot = (int16_t) pkt.GetSlot(0);
136 right_dot = (int16_t) pkt.GetSlot(1);
137 yaw_dot = (int16_t) pkt.GetSlot(2);
138 frames = pkt.GetSlot(3);
139 break;
140
141 case RMP_CAN_ID_MSG4:
142 left = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) |
143 (uint32_t)pkt.GetSlot(0));
144 right = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) |
145 (uint32_t)pkt.GetSlot(2));
146 break;
147
148 case RMP_CAN_ID_MSG5:
149 foreaft = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) |
150 (uint32_t)pkt.GetSlot(0));
151 yaw = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) |
152 (uint32_t)pkt.GetSlot(2));
153 break;
154 case RMP_CAN_ID_MSG6:
155 left_torque = (int16_t) pkt.GetSlot(0);
156 right_torque = (int16_t) pkt.GetSlot(1);
157 break;
158
159 case RMP_CAN_ID_MSG7:
160 op_mode = (int16_t) pkt.GetSlot(0);
161 gain_schedule = (int16_t) pkt.GetSlot(1);
162 ui_battery = (uint16_t) pkt.GetSlot(2);
163 powerbase_battery = (uint16_t) pkt.GetSlot(3);
164 break;
165
166 case RMP_CAN_ID_MSG8:
167 rec_speed = (int16_t) pkt.GetSlot(0);
168 rec_turn = (int16_t) pkt.GetSlot(1);
169 break;
170 default:
171 known = false;
172 break;
173 }
174
175 // now set the ready flags
176 if(known)
177 ready |= (1 << (pkt.id & 0xFF));
178}
Definition canio.h:46
Definition rmp_frame.h:68