NimbRo ROS Soccer Package
•All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Groups Pages
CM730.h
1 // Hardware interface for the CM730 board
2 // File: CM730.h
3 // Authors: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>
4 // Sebastian Schueller <schuell1@cs.uni-bonn.de>
5 // Comment: This file is suitable for CM730 firmware versions of 0x82 and above (NimbRo-OP specific).
6 
7 // Ensure header is included only once
8 #ifndef CM730_H
9 #define CM730_H
10 
11 // Includes
12 #include <cm730/read_thread.h>
13 #include <cm730/dynamixel.h>
14 #include <config_server/parameter.h>
15 #include <plot_msgs/plot_manager.h>
16 #include <ros/time.h>
17 #include <sys/time.h>
18 #include <stddef.h>
19 #include <stdint.h>
20 #include <vector>
21 
22 // CM730 namespace
23 namespace cm730
24 {
25  // Servo bulk read response struct
26  struct BRData
27  {
28  // Constructor
29  BRData() { std::memset(this, 0, sizeof(BRData)); position = -1; }
30 
31  // Bulk read parameters
32  int id;
33  int length;
34  int startAddress;
35 
36  // Received data
37  int position; // P_PRESENT_POSITION_*
38  };
39 
40  // CM730 bulk read response struct
41  struct BRBoard
42  {
43  // Constructor
44  BRBoard() { std::memset(this, 0, sizeof(BRBoard)); }
45 
46  // Bulk read parameters
47  int id;
48  int length;
49  int startAddress;
50 
51  // Received data
52  unsigned char power; // P_DYNAMIXEL_POWER
53  unsigned char ledPanel; // P_LED_PANEL
54  unsigned short rgbled5; // P_RGBLED5_*
55  unsigned short rgbled6; // P_RGBLED6_*
56  unsigned char button; // P_BUTTON
57  unsigned char voltage; // P_BATTERY_VOLTAGE
58  int gyroX; // P_GYRO_X_*
59  int gyroY; // P_GYRO_Y_*
60  int gyroZ; // P_GYRO_Z_*
61  int accX; // P_ACC_X_*
62  int accY; // P_ACC_Y_*
63  int accZ; // P_ACC_Z_*
64  int magX; // P_MAG_X_*
65  int magY; // P_MAG_Y_*
66  int magZ; // P_MAG_Z_*
67  int temp; // P_TEMPERATURE
68  };
69 
70  //
71  // CM730 class
72  //
73  class CM730
74  {
75  public:
76  // Dynamixel communication success enumeration
77  enum
78  {
79  RET_BAD_PARAM = -1,
80  RET_SUCCESS = 0,
81  RET_TX_CORRUPT = 1,
82  RET_TX_FAIL = 2,
83  RET_RX_FAIL = 3,
84  RET_RX_TIMEOUT = 4,
85  RET_RX_CORRUPT = 5,
86  RET_MISC_FAIL = 6
87  };
88 
89  // Dynamixel power states
90  enum DynPowState
91  {
92  DYNPOW_UNKNOWN = -1,
93  DYNPOW_OFF = 0,
94  DYNPOW_ON = 1,
95  DYNPOW_ON_NODXLTX = 2,
96  DYNPOW_ON_MPACKET = 3,
97  DYNPOW_MIN = DYNPOW_OFF,
98  DYNPOW_MAX = DYNPOW_ON_MPACKET
99  };
100 
101  // Constructor/destructor
102  CM730();
103  virtual ~CM730();
104 
105  // Connection functions
106  int connect();
107  int ping(int id, struct timespec* abstime = NULL);
108  void suspend(double suspendTime);
109  void unsuspend();
110 
111  // Send instruction function
112  int sendInstruction(int id, int inst);
113 
114  // Read functions
115  int readByte(int id, int address, int* value, struct timespec* abstime = NULL);
116  int readWord(int id, int address, int* value, struct timespec* abstime = NULL);
117  int readData(int id, int address, void* data, size_t size, struct timespec* abstime = NULL);
118 
119  // Bulk read functions
120  int readCM730(BRBoard* cm730Data);
121  int bulkRead(std::vector<BRData>* servoData, BRBoard* cm730Data);
122  int updateTxBRPacket(const std::vector<int>& servos);
123  bool gotCM730Data() const { return m_gotCM730Data; } // Reset at the beginning of every bulk read
124 
125  // Write functions
126  int writeByte(int id, int address, int value);
127  int writeWord(int id, int address, int value);
128  int writeData(int id, int address, void* data, size_t size);
129  int syncWrite(int address, size_t numDataBytes, size_t numDevices, const uint8_t* data);
130 
131  // Special registers
132  int setDynamixelPower(int value);
133  int getDynamixelPower(DynPowState& state, struct timespec* abstime = NULL);
134  int getButtonState(int& value, struct timespec* abstime = NULL);
135  int beep(float duration, int freq);
136  int sound(int musicIndex);
137  int stopBeep() { return stopSound(); }
138  int stopSound() { return writeByte(ID_CM730, P_BUZZER_PLAY_LENGTH, 0); }
139 
140  // Get functions
141  inline bool isSuspended() { if(m_isSuspended && !m_unsuspendTime.isZero() && (ros::Time::now() >= m_unsuspendTime)) { unsuspend(); } return m_isSuspended; }
142  inline int lastFailedID() const { return m_lastFailedID; }
143  inline int servoFailCount(int id) const { return ((id >= 0) && (id < MAX_DEVICES) ? m_failCount[id] : -1); }
144 
145  protected:
146  // Constants
147  static const char* PATH; // Path to the CM730 device
148  static const int BAUDRATE = 1000000; // 1 Mbps
149  static const int MAX_TX_BYTES = 256; // Tx buffer size of 256 bytes
150  static const int MAX_RX_BYTES = 1024; // Rx buffer size of 1024 bytes
151  static const int MAX_DEVICES = 256; // Maximum number of devices on the bus (importantly, no device ID can exceed this value)
152 
153  // Allow overriding of a current suspension
154  inline void pauseSuspend() { m_wasSuspended = m_isSuspended; m_isSuspended = false; }
155  inline void resumeSuspend() { m_isSuspended = m_wasSuspended; }
156 
157  // Communications functions
158  int txPacket(unsigned char* txp);
159  int rxPacket(unsigned char* rxp, int size, struct timespec* abstime = NULL);
160  void flushPort();
161 
162  // Packet helper functions
163  bool parseBRPacket(unsigned char* rxp, int size, std::vector<BRData>* servoData, BRBoard* cm730Data);
164  bool parseCM730Data(unsigned char* data, int size, BRBoard* cm730Data);
165  int syncRxPacket(unsigned char* rxp, int* readSize);
166  unsigned char checksum(unsigned char* packet);
167 
168  // Plotting
169  plot_msgs::PlotManagerFS m_PM;
170 
171  // Config server parameters
172  config_server::Parameter<bool> m_useBulkReadShortcut; // Parameter specifying whether to make use of the repeat last bulk read command or not (Note: If direct PC->DXL byte feedthrough is enabled in the CM730 firmware, then this should be FALSE, else TRUE, and that's important!)
173 
174  // Variables
175  int m_fd;
176  int m_lastFailedID;
177  io::ReadThread m_readThread;
178  pthread_t m_readThread_thread;
179  unsigned char m_TxBulkRead[MAX_TX_BYTES];
180  bool m_fullBRPacket;
181  ros::Time m_lastFullBRPacket;
182  int m_failCount[MAX_DEVICES];
183  bool m_isSuspended;
184  bool m_wasSuspended;
185  ros::Time m_unsuspendTime;
186  bool m_gotCM730Data;
187  int m_lastSeenDynPow;
188 
189  public:
190  // Dynamixel packet byte positions
191  enum
192  {
193  DP_STARTFF1 = 0,
194  DP_STARTFF2 = 1,
195  DP_ID = 2,
196  DP_LENGTH = 3,
197  DP_INSTRUCTION = 4,
198  DP_ERRFLAGS = 4,
199  DP_PARAMETER = 5
200  };
201 
202  // Dynamixel packet IDs (DP_ID byte)
203  enum
204  {
205  ID_NONE = 0,
206  ID_CM730 = 200,
207  ID_BROADCAST = 254,
208  ID_MIN = 1,
209  ID_MAX = 253
210  };
211 
212  // Dynamixel packet instructions (DP_INSTRUCTION byte)
213  enum
214  {
215  INST_NONE = 0x00,
216  INST_PING = 0x01,
217  INST_READ = 0x02,
218  INST_WRITE = 0x03,
219  INST_REG_WRITE = 0x04,
220  INST_ACTION = 0x05,
221  INST_RESET = 0x06,
222  INST_DIGITAL_RESET = 0x07,
223  INST_SYSTEM_READ = 0x0C,
224  INST_SYSTEM_WRITE = 0x0D,
225  INST_REPEAT_BULK = 0x0F,
226  INST_MEGA_CONFIG = 0x68,
227  INST_MEGA_WRITE = 0x69,
228  INST_MEGA_READ = 0x6A,
229  INST_SYNC_WRITE = 0x83,
230  INST_SYNC_REG_WRITE = 0x84,
231  INST_BULK_READ = 0x92
232  };
233 
234  // Dynamixel packet error flags (DP_ERRFLAGS byte)
235  enum
236  {
237  ERRBIT_VOLTAGE = 0x01,
238  ERRBIT_ANGLE_LIMIT = 0x02,
239  ERRBIT_OVERHEATING = 0x04,
240  ERRBIT_RANGE = 0x08,
241  ERRBIT_CHECKSUM = 0x10,
242  ERRBIT_OVERLOAD = 0x20,
243  ERRBIT_INSTRUCTION = 0x40
244  };
245 
246  // CM730 register map (should match up with the defines in CM_DXL_COM.h, part of the CM730 firmware version 0x81 and above)
247  enum
248  {
249  P_MODEL_NUMBER_L = 0,
250  P_MODEL_NUMBER_H = 1,
251  P_VERSION = 2,
252  P_ID = 3,
253  P_BAUD_RATE = 4,
254  P_RETURN_DELAY_TIME = 5,
255  P_VOLTAGE_LOWER_LIMIT = 12,
256  P_VOLTAGE_UPPER_LIMIT = 13,
257  P_RETURN_LEVEL = 16,
258  P_ALARM_LED = 17,
259  P_DYNAMIXEL_POWER = 24,
260  P_LED_PANEL = 25,
261  P_RGBLED5_L = 26,
262  P_RGBLED5_H = 27,
263  P_RGBLED6_L = 28,
264  P_RGBLED6_H = 29,
265  P_BUTTON = 30,
266  P_BATTERY_VOLTAGE = 31,
267  P_GYRO_X_L = 32,
268  P_GYRO_X_H = 33,
269  P_GYRO_Y_L = 34,
270  P_GYRO_Y_H = 35,
271  P_GYRO_Z_L = 36,
272  P_GYRO_Z_H = 37,
273  P_ACC_X_L = 38,
274  P_ACC_X_H = 39,
275  P_ACC_Y_L = 40,
276  P_ACC_Y_H = 41,
277  P_ACC_Z_L = 42,
278  P_ACC_Z_H = 43,
279  P_MAG_X_L = 44,
280  P_MAG_X_H = 45,
281  P_MAG_Y_L = 46,
282  P_MAG_Y_H = 47,
283  P_MAG_Z_L = 48,
284  P_MAG_Z_H = 49,
285  P_TEMPERATURE = 50,
286  P_ADC0_BATTERY_L = 51,
287  P_ADC0_BATTERY_H = 52,
288  P_ADC1_MIC1_L = 53,
289  P_ADC1_MIC1_H = 54,
290  P_ADC2_MIC2_L = 55,
291  P_ADC2_MIC2_H = 56,
292  P_ADC3_L = 57,
293  P_ADC3_H = 58,
294  P_ADC4_L = 59,
295  P_ADC4_H = 60,
296  P_ADC5_L = 61,
297  P_ADC5_H = 62,
298  P_ADC6_L = 63,
299  P_ADC6_H = 64,
300  P_ADC7_L = 65,
301  P_ADC7_H = 66,
302  P_ADC8_L = 67,
303  P_ADC8_H = 68,
304  P_ADC9_L = 69,
305  P_ADC9_H = 70,
306  P_ADC10_L = 71,
307  P_ADC10_H = 72,
308  P_ADC11_L = 73,
309  P_ADC11_H = 74,
310  P_ADC12_L = 75,
311  P_ADC12_H = 76,
312  P_ADC13_L = 77,
313  P_ADC13_H = 78,
314  P_ADC14_L = 79,
315  P_ADC14_H = 80,
316  P_ADC15_L = 81,
317  P_ADC15_H = 82,
318  P_BUZZER_PLAY_LENGTH = 83,
319  P_BUZZER_DATA = 84,
320  P_DXLRX_PACKET_CNT_L = 91,
321  P_DXLRX_PACKET_CNT_H = 92,
322  P_DXLRX_OVERFLOW_CNT_L = 93,
323  P_DXLRX_OVERFLOW_CNT_H = 94,
324  P_DXLRX_BUFERROR_CNT_L = 95,
325  P_DXLRX_BUFERROR_CNT_H = 96,
326  P_DXLRX_CHKERROR_CNT_L = 97,
327  P_DXLRX_CHKERROR_CNT_H = 98,
328  P_DXLRX_ORE_CNT_L = 99,
329  P_DXLRX_ORE_CNT_H = 100,
330  P_DXLTX_PACKET_CNT_L = 101,
331  P_DXLTX_PACKET_CNT_H = 102,
332  P_DXLTX_OVERFLOW_CNT_L = 103,
333  P_DXLTX_OVERFLOW_CNT_H = 104,
334  P_DXLTX_BUFERROR_CNT_L = 105,
335  P_DXLTX_BUFERROR_CNT_H = 106,
336  P_PCRX_PACKET_CNT_L = 107,
337  P_PCRX_PACKET_CNT_H = 108,
338  P_PCRX_OVERFLOW_CNT_L = 109,
339  P_PCRX_OVERFLOW_CNT_H = 110,
340  P_PCRX_BUFERROR_CNT_L = 111,
341  P_PCRX_BUFERROR_CNT_H = 112,
342  P_PCRX_CHKERROR_CNT_L = 113,
343  P_PCRX_CHKERROR_CNT_H = 114,
344  P_PCRX_ORE_CNT_L = 115,
345  P_PCRX_ORE_CNT_H = 116,
346  P_PCTX_PACKET_CNT_L = 117,
347  P_PCTX_PACKET_CNT_H = 118,
348  P_PCTX_OVERFLOW_CNT_L = 119,
349  P_PCTX_OVERFLOW_CNT_H = 120,
350  P_PCTX_BUFERROR_CNT_L = 121,
351  P_PCTX_BUFERROR_CNT_H = 122,
352  P_MISC0 = 123,
353  P_MISC1 = 124,
354  P_MISC2 = 125,
355  P_MISC3 = 126,
356  CM730_REGISTER_NUM
357  };
358 
359  // Bulk read addresses
360  enum
361  {
362  READ_CM730_ADDRESS = CM730::P_DYNAMIXEL_POWER, // CM730: Read the address range P_DYNAMIXEL_POWER (24) --> P_TEMPERATURE (50) = 27 bytes
363  READ_CM730_LENGTH = CM730::P_TEMPERATURE - READ_CM730_ADDRESS + 1,
364  READ_SERVO_ADDRESS = DynamixelMX::P_PRESENT_POSITION_L, // Read the address range P_PRESENT_POSITION_L (36) --> P_PRESENT_POSITION_H (37) = 2 bytes
365  READ_SERVO_LENGTH = DynamixelMX::P_PRESENT_POSITION_H - READ_SERVO_ADDRESS + 1
366  };
367  };
368 }
369 
370 #endif
371 // EOF