23 #include "canio_kvaser.h"
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
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
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 | \
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
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
67 #define RMP_GEOM_WHEEL_SEP 0.54
90 int16_t gain_schedule;
92 uint16_t powerbase_battery;
105 bool IsReady() {
return ready == 0xFF; }
120 rmp_frame_t::AddPacket(
const CanPacket &pkt)
126 case RMP_CAN_ID_MSG1:
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);
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);
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));
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));
157 case RMP_CAN_ID_MSG6:
158 left_torque = (int16_t) pkt.GetSlot(0);
159 right_torque = (int16_t) pkt.GetSlot(1);
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);
169 case RMP_CAN_ID_MSG8:
170 rec_speed = (int16_t) pkt.GetSlot(0);
171 rec_turn = (int16_t) pkt.GetSlot(1);
180 ready |= (1 << (pkt.id & 0xFF));
Definition: rmp_frame.h:70