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
67 class rmp_frame_t
68 {
69 public:
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  */
116 inline void
117 rmp_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 }
CanPacket
Definition: canio.h:45
rmp_frame_t
Definition: rmp_frame.h:67
Generated on Wed Sep 2 2020 16:39:28 for Player by doxygen 1.8.13