This repository has been archived by the owner on May 17, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 23
/
Dynamixel_Serial.h
executable file
·220 lines (181 loc) · 8.38 KB
/
Dynamixel_Serial.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
/*
Version 2.2
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#ifndef Dynamixel_Serial_h
#define Dynamixel_Serial_h
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#include <pins_arduino.h>
#endif
//#########################################################################
//################ define - Dynamixel Hex code table ######################
// EEPROM AREA
#define EEPROM_MODEL_NUMBER_L 0x00
#define EEPROM_MODEL_NUMBER_H 0x01
#define EEPROM_VERSION 0x02
#define EEPROM_ID 0x03
#define EEPROM_BAUD_RATE 0x04
#define EEPROM_RETURN_DELAY_TIME 0x05
#define EEPROM_CW_ANGLE_LIMIT_L 0x06
#define EEPROM_CW_ANGLE_LIMIT_H 0x07
#define EEPROM_CCW_ANGLE_LIMIT_L 0x08
#define EEPROM_CCW_ANGLE_LIMIT_H 0x09
#define EEPROM_LIMIT_TEMPERATURE 0x0B
#define EEPROM_LOW_LIMIT_VOLTAGE 0x0C
#define EEPROM_HIGN_LIMIT_VOLTAGE 0x0D
#define EEPROM_MAX_TORQUE_L 0x0E
#define EEPROM_MAX_TORQUE_H 0x0F
#define EEPROM_RETURN_LEVEL 0x10
#define EEPROM_ALARM_LED 0x11
#define EEPROM_ALARM_SHUTDOWN 0x12
// RAM AREA
#define RAM_TORQUE_ENABLE 0x18
#define RAM_LED 0x19
#define RAM_PROPORTIONAL_GAIN 0x1C
#define RAM_INTERGRAL_GAIN 0x1B
#define RAM_DERIVATIVE_GAIN 0x1A
#define RAM_GOAL_POSITION_L 0x1E
#define RAM_GOAL_POSITION_H 0x1F
#define RAM_GOAL_SPEED_L 0x20
#define RAM_GOAL_SPEED_H 0x21
#define RAM_TORQUE_LIMIT_L 0x22
#define RAM_TORQUE_LIMIT_H 0x23
#define RAM_PRESENT_POSITION_L 0x24
#define RAM_PRESENT_POSITION_H 0x25
#define RAM_PRESENT_SPEED_L 0x26
#define RAM_PRESENT_SPEED_H 0x27
#define RAM_PRESENT_LOAD_L 0x28
#define RAM_PRESENT_LOAD_H 0x29
#define RAM_PRESENT_VOLTAGE 0x2A
#define RAM_PRESENT_TEMPERATURE 0x2B
#define RAM_REGISTER 0x2C
#define RAM_MOVING 0x2E
#define RAM_LOCK 0x2F
#define RAM_PUNCH_L 0x30
#define RAM_PUNCH_H 0x31
//#########################################################################
//################ Instruction commands Set ###############################
#define COMMAND_PING 0x01
#define COMMAND_READ_DATA 0x02
#define COMMAND_WRITE_DATA 0x03
#define COMMAND_REG_WRITE_DATA 0x04
#define COMMAND_ACTION 0x05
#define COMMAND_RESET 0x06
#define COMMAND_SYNC_WRITE 0x83
//#########################################################################
//################ Instruction packet lengths #############################
// Packet length is number of parameters (N) + 2
#define READ_ONE_BYTE_LENGTH 0x01
#define READ_TWO_BYTE_LENGTH 0x02
#define RESET_LENGTH 0x02
#define PING_LENGTH 0x02
#define ACTION_LENGTH 0x02
#define SET_ID_LENGTH 0x04
#define SET_BD_LENGTH 0x04
#define SET_RETURN_LEVEL_LENGTH 0x04
#define READ_TEMP_LENGTH 0x04
#define READ_POS_LENGTH 0x04
#define READ_LOAD_LENGTH 0x04
#define READ_SPEED_LENGTH 0x04
#define READ_VOLT_LENGTH 0x04
#define READ_REGISTER_LENGTH 0x04
#define READ_MOVING_LENGTH 0x04
#define READ_LOCK_LENGTH 0x04
#define LED_LENGTH 0x04
#define SET_HOLDING_TORQUE_LENGTH 0x04
#define SET_MAX_TORQUE_LENGTH 0x05
#define SET_ALARM_LENGTH 0x04
#define READ_LOAD_LENGTH 0x04
#define SET_RETURN_LENGTH 0x04
#define WHEEL_LENGTH 0x05
#define SERVO_GOAL_LENGTH 0x07
#define SET_MODE_LENGTH 0x07
#define SET_PUNCH_LENGTH 0x04
#define SET_PID_LENGTH 0x06
#define SET_TEMP_LENGTH 0x04
#define SET_VOLT_LENGTH 0x05
#define SYNC_LOAD_LENGTH 0x0D
#define SYNC_DATA_LENGTH 0x02
//#########################################################################
//############################ Specials ###################################
#define PORT0 0x00
#define PORT1 0x01
#define PORT2 0x02
#define PORT3 0x03
#define OFF 0x00
#define ON 0x01
#define SERVO 0x01
#define WHEEL 0x00
#define LEFT 0x00
#define RIGHT 0x01
#define NONE 0x00
#define READ 0x01
#define ALL 0x02
#define BROADCAST_ID 0xFE
#define HEADER 0xFF
#define STATUS_PACKET_TIMEOUT 50 // in millis()
#define STATUS_FRAME_BUFFER 5
class DynamixelClass {
public:
// Constructor
DynamixelClass(): Direction_Pin(-1), Status_Return_Value(READ) { }
void begin(long);
void begin(HardwareSerial&, long);
void begin(Stream&);
void end(void);
void setDirectionPin(unsigned char);
unsigned int reset(unsigned char);
unsigned int ping(unsigned char);
unsigned int setStatusPaketReturnDelay(unsigned char,unsigned char);
unsigned int setID(unsigned char, unsigned char);
unsigned int setBaudRate(unsigned char, long);
unsigned int setMaxTorque(unsigned char, int);
unsigned int setHoldingTorque(unsigned char, bool);
unsigned int setAlarmShutdown(unsigned char,unsigned char);
unsigned int setStatusPaket(unsigned char,unsigned char);
unsigned int setMode(unsigned char, bool, unsigned int, unsigned int);
unsigned int setPunch(unsigned char, unsigned int);
unsigned int setPID(unsigned char, unsigned char, unsigned char, unsigned char);
unsigned int setTemp(unsigned char, unsigned char);
unsigned int setVoltage(unsigned char, unsigned char, unsigned char);
unsigned int servo(unsigned char, unsigned int, unsigned int);
unsigned int servoPreload(unsigned char, unsigned int, unsigned int);
unsigned int wheel(unsigned char, bool, unsigned int);
void wheelSync(unsigned char, bool, unsigned int, unsigned char, bool, unsigned int, unsigned char, bool, unsigned int);
unsigned int wheelPreload(unsigned char, bool, unsigned int);
unsigned int action(unsigned char);
unsigned int readTemperature(unsigned char);
unsigned int readVoltage(unsigned char);
unsigned int readPosition(unsigned char);
unsigned int readLoad(unsigned char);
unsigned int readSpeed(unsigned char);
unsigned int checkRegister(unsigned char);
unsigned int checkMovement(unsigned char);
unsigned int checkLock(unsigned char);
unsigned int ledState(unsigned char, bool);
private:
void transmitInstructionPacket(void);
unsigned int readStatusPacket(void);
void clearRXbuffer(void);
Stream *_serial;
unsigned char Instruction_Packet_Array[14]; // Array to hold instruction packet data
unsigned char Status_Packet_Array[8]; // Array to hold returned status packet data
unsigned long Time_Counter; // Timer for time out watchers
char Direction_Pin; // Pin to control TX/RX buffer chip
unsigned char Status_Return_Value; // Status packet return states ( NON , READ , ALL )
};
extern DynamixelClass Dynamixel;
#endif