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., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18  *
19  */
20 
21 
22 #include "canio.h"
23 #include "canio_kvaser.h"
24 
25 
26 #define RMP_CAN_ID_SHUTDOWN 0x0412
27 #define RMP_CAN_ID_COMMAND 0x0413
28 #define RMP_CAN_ID_MSG1 0x0400
29 #define RMP_CAN_ID_MSG2 0x0401
30 #define RMP_CAN_ID_MSG3 0x0402
31 #define RMP_CAN_ID_MSG4 0x0403
32 #define RMP_CAN_ID_MSG5 0x0404
33 #define RMP_CAN_ID_MSG6 0x0405
34 #define RMP_CAN_ID_MSG7 0x0406
35 #define RMP_CAN_ID_MSG8 0x0407
36 
37 #define RMP_CAN_CMD_NONE 0
38 #define RMP_CAN_CMD_MAX_VEL 10
39 #define RMP_CAN_CMD_MAX_ACCL 11
40 #define RMP_CAN_CMD_MAX_TURN 12
41 #define RMP_CAN_CMD_GAIN_SCHED 13
42 #define RMP_CAN_CMD_CURR_LIMIT 14
43 #define RMP_CAN_CMD_RST_INT 50
44 
45 #define RMP_CAN_RST_RIGHT 0x01
46 #define RMP_CAN_RST_LEFT 0x02
47 #define RMP_CAN_RST_YAW 0x04
48 #define RMP_CAN_RST_FOREAFT 0x08
49 #define RMP_CAN_RST_ALL (RMP_CAN_RST_RIGHT | \
50  RMP_CAN_RST_LEFT | \
51  RMP_CAN_RST_YAW | \
52  RMP_CAN_RST_FOREAFT)
53 
54 #define RMP_COUNT_PER_M 33215
55 #define RMP_COUNT_PER_DEG 7.8
56 #define RMP_COUNT_PER_M_PER_S 332
57 #define RMP_COUNT_PER_DEG_PER_S 7.8
58 #define RMP_COUNT_PER_MM_PER_S 0.32882963
59 #define RMP_COUNT_PER_DEG_PER_SS 7.8
60 #define RMP_COUNT_PER_REV 112644
61 
62 #define RMP_MAX_TRANS_VEL_MM_S 3576
63 #define RMP_MAX_ROT_VEL_DEG_S 18 // from rmi_demo: 1300*0.013805056
64 #define RMP_MAX_TRANS_VEL_COUNT 1176
65 #define RMP_MAX_ROT_VEL_COUNT 1024
66 
67 #define RMP_GEOM_WHEEL_SEP 0.54
68 
69 // this holds all the RMP data it gives us
71 {
72 public:
73  int16_t pitch;
74  int16_t pitch_dot;
75  int16_t roll;
76  int16_t roll_dot;
77  uint32_t yaw;
78  int16_t yaw_dot;
79  uint32_t left;
80  int16_t left_dot;
81  uint32_t right;
82  int16_t right_dot;
83  uint32_t foreaft;
84 
85  uint16_t frames;
86  int16_t left_torque;
87  int16_t right_torque;
88 
89  int16_t op_mode;
90  int16_t gain_schedule;
91  uint16_t ui_battery;
92  uint16_t powerbase_battery;
93 
94  int16_t rec_speed;
95  int16_t rec_turn;
96 
97  uint8_t ready;
98 
99  rmp_frame_t() : ready(0) {}
100 
101  // Adds a new packet to this frame
102  void AddPacket(const CanPacket &pkt);
103 
104  // Is this frame ready (i.e., did we get all 5 messages)?
105  bool IsReady() { return ready == 0xFF; }
106 
107 };
108 
109 
110 
111 /* Takes a CAN packet from the RMP and parses it into a
112  * rmp_frame_t struct. sets the ready bitfield
113  * depending on which CAN packet we have. when
114  * ready == 0x1F, then we have gotten 5 packets, so everything
115  * is filled in.
116  *
117  * returns:
118  */
119 inline void
120 rmp_frame_t::AddPacket(const CanPacket &pkt)
121 {
122  bool known = true;
123 
124  switch(pkt.id)
125  {
126  case RMP_CAN_ID_MSG1:
127  //powerbase_battery = pkt.GetSlot(2);
128  break;
129 
130  case RMP_CAN_ID_MSG2:
131  pitch = pkt.GetSlot(0);
132  pitch_dot = pkt.GetSlot(1);
133  roll = pkt.GetSlot(2);
134  roll_dot = pkt.GetSlot(3);
135  break;
136 
137  case RMP_CAN_ID_MSG3:
138  left_dot = (int16_t) pkt.GetSlot(0);
139  right_dot = (int16_t) pkt.GetSlot(1);
140  yaw_dot = (int16_t) pkt.GetSlot(2);
141  frames = pkt.GetSlot(3);
142  break;
143 
144  case RMP_CAN_ID_MSG4:
145  left = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) |
146  (uint32_t)pkt.GetSlot(0));
147  right = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) |
148  (uint32_t)pkt.GetSlot(2));
149  break;
150 
151  case RMP_CAN_ID_MSG5:
152  foreaft = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) |
153  (uint32_t)pkt.GetSlot(0));
154  yaw = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) |
155  (uint32_t)pkt.GetSlot(2));
156  break;
157  case RMP_CAN_ID_MSG6:
158  left_torque = (int16_t) pkt.GetSlot(0);
159  right_torque = (int16_t) pkt.GetSlot(1);
160  break;
161 
162  case RMP_CAN_ID_MSG7:
163  op_mode = (int16_t) pkt.GetSlot(0);
164  gain_schedule = (int16_t) pkt.GetSlot(1);
165  ui_battery = (uint16_t) pkt.GetSlot(2);
166  powerbase_battery = (uint16_t) pkt.GetSlot(3);
167  break;
168 
169  case RMP_CAN_ID_MSG8:
170  rec_speed = (int16_t) pkt.GetSlot(0);
171  rec_turn = (int16_t) pkt.GetSlot(1);
172  break;
173  default:
174  known = false;
175  break;
176  }
177 
178  // now set the ready flags
179  if(known)
180  ready |= (1 << (pkt.id & 0xFF));
181 }

Last updated 12 September 2005 21:38:45