rmp_frame.h
00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "canio.h"
00023 #include "canio_kvaser.h"
00024
00025
00026 #define RMP_CAN_ID_SHUTDOWN 0x0412
00027 #define RMP_CAN_ID_COMMAND 0x0413
00028 #define RMP_CAN_ID_MSG1 0x0400
00029 #define RMP_CAN_ID_MSG2 0x0401
00030 #define RMP_CAN_ID_MSG3 0x0402
00031 #define RMP_CAN_ID_MSG4 0x0403
00032 #define RMP_CAN_ID_MSG5 0x0404
00033 #define RMP_CAN_ID_MSG6 0x0405
00034 #define RMP_CAN_ID_MSG7 0x0406
00035 #define RMP_CAN_ID_MSG8 0x0407
00036
00037 #define RMP_CAN_CMD_NONE 0
00038 #define RMP_CAN_CMD_MAX_VEL 10
00039 #define RMP_CAN_CMD_MAX_ACCL 11
00040 #define RMP_CAN_CMD_MAX_TURN 12
00041 #define RMP_CAN_CMD_GAIN_SCHED 13
00042 #define RMP_CAN_CMD_CURR_LIMIT 14
00043 #define RMP_CAN_CMD_RST_INT 50
00044
00045 #define RMP_CAN_RST_RIGHT 0x01
00046 #define RMP_CAN_RST_LEFT 0x02
00047 #define RMP_CAN_RST_YAW 0x04
00048 #define RMP_CAN_RST_FOREAFT 0x08
00049 #define RMP_CAN_RST_ALL (RMP_CAN_RST_RIGHT | \
00050 RMP_CAN_RST_LEFT | \
00051 RMP_CAN_RST_YAW | \
00052 RMP_CAN_RST_FOREAFT)
00053
00054 #define RMP_COUNT_PER_M 33215
00055 #define RMP_COUNT_PER_DEG 7.8
00056 #define RMP_COUNT_PER_M_PER_S 332
00057 #define RMP_COUNT_PER_DEG_PER_S 7.8
00058 #define RMP_COUNT_PER_MM_PER_S 0.32882963
00059 #define RMP_COUNT_PER_DEG_PER_SS 7.8
00060 #define RMP_COUNT_PER_REV 112644
00061
00062 #define RMP_MAX_TRANS_VEL_MM_S 3576
00063 #define RMP_MAX_ROT_VEL_DEG_S 18 // from rmi_demo: 1300*0.013805056
00064 #define RMP_MAX_TRANS_VEL_COUNT 1176
00065 #define RMP_MAX_ROT_VEL_COUNT 1024
00066
00067 #define RMP_GEOM_WHEEL_SEP 0.54
00068
00069
00070 class rmp_frame_t
00071 {
00072 public:
00073 int16_t pitch;
00074 int16_t pitch_dot;
00075 int16_t roll;
00076 int16_t roll_dot;
00077 uint32_t yaw;
00078 int16_t yaw_dot;
00079 uint32_t left;
00080 int16_t left_dot;
00081 uint32_t right;
00082 int16_t right_dot;
00083 uint32_t foreaft;
00084
00085 uint16_t frames;
00086 int16_t left_torque;
00087 int16_t right_torque;
00088
00089 int16_t op_mode;
00090 int16_t gain_schedule;
00091 uint16_t ui_battery;
00092 uint16_t powerbase_battery;
00093
00094 int16_t rec_speed;
00095 int16_t rec_turn;
00096
00097 uint8_t ready;
00098
00099 rmp_frame_t() : ready(0) {}
00100
00101
00102 void AddPacket(const CanPacket &pkt);
00103
00104
00105 bool IsReady() { return ready == 0xFF; }
00106
00107 };
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119 inline void
00120 rmp_frame_t::AddPacket(const CanPacket &pkt)
00121 {
00122 bool known = true;
00123
00124 switch(pkt.id)
00125 {
00126 case RMP_CAN_ID_MSG1:
00127
00128 break;
00129
00130 case RMP_CAN_ID_MSG2:
00131 pitch = pkt.GetSlot(0);
00132 pitch_dot = pkt.GetSlot(1);
00133 roll = pkt.GetSlot(2);
00134 roll_dot = pkt.GetSlot(3);
00135 break;
00136
00137 case RMP_CAN_ID_MSG3:
00138 left_dot = (int16_t) pkt.GetSlot(0);
00139 right_dot = (int16_t) pkt.GetSlot(1);
00140 yaw_dot = (int16_t) pkt.GetSlot(2);
00141 frames = pkt.GetSlot(3);
00142 break;
00143
00144 case RMP_CAN_ID_MSG4:
00145 left = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) |
00146 (uint32_t)pkt.GetSlot(0));
00147 right = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) |
00148 (uint32_t)pkt.GetSlot(2));
00149 break;
00150
00151 case RMP_CAN_ID_MSG5:
00152 foreaft = (uint32_t)(((uint32_t)pkt.GetSlot(1) << 16) |
00153 (uint32_t)pkt.GetSlot(0));
00154 yaw = (uint32_t)(((uint32_t)pkt.GetSlot(3) << 16) |
00155 (uint32_t)pkt.GetSlot(2));
00156 break;
00157 case RMP_CAN_ID_MSG6:
00158 left_torque = (int16_t) pkt.GetSlot(0);
00159 right_torque = (int16_t) pkt.GetSlot(1);
00160 break;
00161
00162 case RMP_CAN_ID_MSG7:
00163 op_mode = (int16_t) pkt.GetSlot(0);
00164 gain_schedule = (int16_t) pkt.GetSlot(1);
00165 ui_battery = (uint16_t) pkt.GetSlot(2);
00166 powerbase_battery = (uint16_t) pkt.GetSlot(3);
00167 break;
00168
00169 case RMP_CAN_ID_MSG8:
00170 rec_speed = (int16_t) pkt.GetSlot(0);
00171 rec_turn = (int16_t) pkt.GetSlot(1);
00172 break;
00173 default:
00174 known = false;
00175 break;
00176 }
00177
00178
00179 if(known)
00180 ready |= (1 << (pkt.id & 0xFF));
00181 }