Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added comment to provide information on AX MX default use of protocol 1 communications. #124

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,8 @@ class NewSerialPortHandler : public DYNAMIXEL::SerialPortHandler
};

const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl;
Expand Down
2 changes: 2 additions & 0 deletions examples/advanced/fast_sync_read/fast_sync_read.ino
Original file line number Diff line number Diff line change
Expand Up @@ -94,6 +94,8 @@
} __attribute__((packed));
*/

// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DYNAMIXEL_PROTOCOL_VERSION = 1.0;
const float DYNAMIXEL_PROTOCOL_VERSION = 2.0;
const uint8_t BROADCAST_ID = 254;
const uint8_t DXL_ID_CNT = 2;
Expand Down
2 changes: 2 additions & 0 deletions examples/advanced/indirect_address/indirect_address.ino
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,8 @@


const uint8_t BROADCAST_ID = 254;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DYNAMIXEL_PROTOCOL_VERSION = 1.0;
const float DYNAMIXEL_PROTOCOL_VERSION = 2.0;

const uint8_t DXL_ID_CNT = 2;
Expand Down
2 changes: 2 additions & 0 deletions examples/advanced/pid_tuning/pid_tuning.ino
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,8 @@

const uint8_t DXL_ID = 1;
const uint32_t DXL_BAUDRATE = 57600;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DYNAMIXEL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

int32_t goal_position[2] = {1200, 1600};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -162,6 +162,8 @@ using namespace ControlTableItem;
void setup() {
// put your setup code here, to run once:
DEBUG_SERIAL.begin(115200);
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: dxl.setPortProtocolVersion(2.0);
dxl.setPortProtocolVersion(2.0);
dxl.begin(57600);
dxl.scan();
Expand Down Expand Up @@ -211,4 +213,4 @@ void loop() {
}

delay(2000);
}
}
4 changes: 3 additions & 1 deletion examples/advanced/slave/slave.ino
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,8 @@ void setup() {
// Speed setting for communication (not necessary for USB)
dxl_port.begin(1000000);

// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: dxl.setPortProtocolVersion(DXL_PROTOCOL_VER_1_0);
dxl.setPortProtocolVersion(DXL_PROTOCOL_VER_2_0);
dxl.setFirmwareVersion(1);
dxl.setID(1);
Expand All @@ -111,4 +113,4 @@ void loop() {
// Update or use variables for each control item.
digitalWrite(LED_BUILTIN, control_item_led);
control_item_analog = analogRead(A0);
}
}
2 changes: 2 additions & 0 deletions examples/advanced/slave_callback/slave_callback.ino
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,8 @@ void setup() {
// Speed setting for communication (not necessary for USB)
dxl_port.begin(1000000);

// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: dxl.setPortProtocolVersion(DXL_PROTOCOL_VER_1_0);
dxl.setPortProtocolVersion(DXL_PROTOCOL_VER_2_0);
dxl.setFirmwareVersion(1);
dxl.setID(1);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,8 @@
*/

const uint8_t BROADCAST_ID = 254;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DYNAMIXEL_PROTOCOL_VERSION = 2.0;
const float DYNAMIXEL_PROTOCOL_VERSION = 2.0;
const uint8_t DXL_ID_CNT = 2;
const uint8_t DXL_ID_LIST[DXL_ID_CNT] = {1, 2};
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,8 @@
*/

const uint8_t BROADCAST_ID = 254;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DYNAMIXEL_PROTOCOL_VERSION = 2.0;
const float DYNAMIXEL_PROTOCOL_VERSION = 2.0;
const uint8_t DXL_ID_CNT = 2;
const uint8_t DXL_ID_LIST[DXL_ID_CNT] = {1, 2};
Expand Down
4 changes: 3 additions & 1 deletion examples/basic/baudrate/baudrate.ino
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@


const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;
uint32_t BAUDRATE = 57600;
uint32_t NEW_BAUDRATE = 1000000; //1Mbsp
Expand Down Expand Up @@ -105,4 +107,4 @@ void setup() {

void loop() {
// put your main code here, to run repeatedly:
}
}
2 changes: 2 additions & 0 deletions examples/basic/current_mode/current_mode.ino
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@


const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,8 @@


const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down
4 changes: 3 additions & 1 deletion examples/basic/id/id.ino
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@


const uint8_t DEFAULT_DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down Expand Up @@ -116,4 +118,4 @@ void setup() {

void loop() {
// put your main code here, to run repeatedly:
}
}
2 changes: 2 additions & 0 deletions examples/basic/led/led.ino
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@


const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down
2 changes: 2 additions & 0 deletions examples/basic/operation_mode/operation_mode.ino
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,8 @@


const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down
4 changes: 3 additions & 1 deletion examples/basic/ping/ping.ino
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@


const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down Expand Up @@ -117,4 +119,4 @@ void FindServos(void) {
Serial.print("Broadcast returned no items : ");
Serial.println(dxl.getLastLibErrCode());
}
}
}
2 changes: 2 additions & 0 deletions examples/basic/position_mode/position_mode.ino
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@


const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,8 @@


const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down
2 changes: 2 additions & 0 deletions examples/basic/pwm_mode/pwm_mode.ino
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@


const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down
2 changes: 2 additions & 0 deletions examples/basic/torque/torque.ino
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,8 @@


const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down
2 changes: 2 additions & 0 deletions examples/basic/velocity_mode/velocity_mode.ino
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@


const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down
4 changes: 3 additions & 1 deletion examples/dynamixel_protocol/factory_reset/factory_reset.ino
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@

#define TIMEOUT 10 //default communication timeout 10ms
const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

bool ret = false;
Expand Down Expand Up @@ -131,4 +133,4 @@ void loop() {
}

delay(1000);
}
}
4 changes: 3 additions & 1 deletion examples/dynamixel_protocol/ping/ping.ino
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,8 @@ uint8_t ret = 0;
uint8_t recv_count = 0;

const uint8_t DXL_ID = BROADCAST_ID;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
Expand Down Expand Up @@ -104,4 +106,4 @@ void setup() {

void loop() {
// put your main code here, to run repeatedly:
}
}
4 changes: 3 additions & 1 deletion examples/dynamixel_protocol/reboot/reboot.ino
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@

#define TIMEOUT 10 //default communication timeout 10ms
const uint8_t DXL_ID = 1;
// MX and AX servos use DYNAMIXEL Protocol 1.0 by default.
// to use MX and AX servos with this example, change the following line to: const float DXL_PROTOCOL_VERSION = 1.0;
const float DXL_PROTOCOL_VERSION = 2.0;
uint8_t option = 0;

Expand Down Expand Up @@ -98,4 +100,4 @@ void loop() {
}

delay(1000);
}
}
6 changes: 3 additions & 3 deletions keywords.txt
Original file line number Diff line number Diff line change
Expand Up @@ -193,9 +193,9 @@ SHUTDOWN LITERAL1

TORQUE_ENABLE LITERAL1
LED LITERAL1
LED_RED LITERAL1
LED_GREEN LITERAL1
LED_BLUE LITERAL1
RGB_LED_RED LITERAL1
RGB_LED_GREEN LITERAL1
RGB_LED_BLUE LITERAL1
REGISTERED_INSTRUCTION LITERAL1
HARDWARE_ERROR_STATUS LITERAL1
VELOCITY_P_GAIN LITERAL1
Expand Down
6 changes: 3 additions & 3 deletions src/Dynamixel2Arduino.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -558,10 +558,10 @@ bool Dynamixel2Arduino::setLedState(uint8_t id, bool state)
case PRO_M54P_040_S250_R:
case PRO_M54P_060_S250_R:
if (state == false) {
writeControlTableItem(ControlTableItem::LED_GREEN, id, state);
writeControlTableItem(ControlTableItem::LED_BLUE, id, state);
writeControlTableItem(ControlTableItem::RGB_LED_GREEN, id, state);
writeControlTableItem(ControlTableItem::RGB_LED_BLUE, id, state);
}
ret = writeControlTableItem(ControlTableItem::LED_RED, id, state);
ret = writeControlTableItem(ControlTableItem::RGB_LED_RED, id, state);
break;

default:
Expand Down
Loading