...
POST No. 2408035
Issue controlling MX-12W using openCM 9.04 and expansion board 485
2021-04-03 20:21:00 medhas

Hi,

I have purchased the AX-12A and MX-12W motors for my project and I tried to control them using OpenCM 9.04 C type combined with the expansion board 485. I also powered the Exp board with 12V, 5A SMPS connection. But when I try to program it using the Arduino IDE, it doesnt seem to work. I have checked with the port and followed youtube videos from ROBOTIS Customer Support. Can you please let me know how should I proceed?

2021-04-03 20:21:00
medhas
2021-04-05 10:58:09 Will Son

Hi,

Could you show me which youtube video did you refer to?

And please let me know which Arduino example did you upload to test your DYNAMIXEL.


Please note that if you have connected DYNAMIXEL to OpenCM9.04 EXP without ID configuration, each DYNAMIXEL is configured to ID 1 which will cause digital communication collision problem.

When configuring DYNAMIXEL for the first time, please connect only one at a time.


If you have installed DYNAMIXEL2Arduino library on your Arduino IDE, you can run below code that I modified from "position_mode" example of DYNAMIXEL2Arduino with a single AX-12A.

Thank you.


/*******************************************************************************
* Copyright 2016 ROBOTIS CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*******************************************************************************/

#include <Dynamixel2Arduino.h>

// Please modify it to suit your hardware.
#if defined(ARDUINO_AVR_UNO) || defined(ARDUINO_AVR_MEGA2560) // When using DynamixelShield
#include <SoftwareSerial.h>
SoftwareSerial soft_serial(7, 8); // DYNAMIXELShield UART RX/TX
#define DXL_SERIAL Serial
#define DEBUG_SERIAL soft_serial
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_SAM_DUE) // When using DynamixelShield
#define DXL_SERIAL Serial
#define DEBUG_SERIAL SerialUSB
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_SAM_ZERO) // When using DynamixelShield
#define DXL_SERIAL Serial1
#define DEBUG_SERIAL SerialUSB
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#elif defined(ARDUINO_OpenCM904) // When using official ROBOTIS board with DXL circuit.
#define DXL_SERIAL Serial3 //OpenCM9.04 EXP Board's DXL port Serial. (Serial1 for the DXL port on the OpenCM 9.04 board)
#define DEBUG_SERIAL Serial
const uint8_t DXL_DIR_PIN = 22; //OpenCM9.04 EXP Board's DIR PIN. (28 for the DXL port on the OpenCM 9.04 board)
#elif defined(ARDUINO_OpenCR) // When using official ROBOTIS board with DXL circuit.
// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it.
#define DXL_SERIAL Serial3
#define DEBUG_SERIAL Serial
const uint8_t DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN.
#else // Other boards when using DynamixelShield
#define DXL_SERIAL Serial1
#define DEBUG_SERIAL Serial
const uint8_t DXL_DIR_PIN = 2; // DYNAMIXEL Shield DIR PIN
#endif

const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 1.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

//This namespace is required to use Control table item names
using namespace ControlTableItem;

void setup() {
// put your setup code here, to run once:
// Use UART port of DYNAMIXEL Shield to debug.
DEBUG_SERIAL.begin(57600);
while(!DEBUG_SERIAL);

// Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate.
dxl.begin(1000000);
// Set Port Protocol Version. This has to match with DYNAMIXEL protocol version.
dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
// Get DYNAMIXEL information
dxl.ping(DXL_ID);

// Turn off torque when configuring items in EEPROM area
dxl.torqueOff(DXL_ID);
dxl.setOperatingMode(DXL_ID, OP_POSITION);
dxl.torqueOn(DXL_ID);
}

void loop() {
// put your main code here, to run repeatedly:
// Please refer to e-Manual(http://emanual.robotis.com/docs/en/parts/interface/dynamixel_shield/) for available range of value.
// Set Goal Position in RAW value
dxl.setGoalPosition(DXL_ID, 512);

int i_present_position = 0;
float f_present_position = 0.0;

while (abs(512 - i_present_position) > 10)
{
f_present_position = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
i_present_position = dxl.getPresentPosition(DXL_ID);
DEBUG_SERIAL.print("Present_Position(raw) : ");
DEBUG_SERIAL.println(i_present_position);
}
delay(500);

// Set Goal Position in DEGREE value
dxl.setGoalPosition(DXL_ID, 5.7, UNIT_DEGREE);
while (abs(5.7 - f_present_position) > 2.0)
{
f_present_position = dxl.getPresentPosition(DXL_ID, UNIT_DEGREE);
i_present_position = dxl.getPresentPosition(DXL_ID);
DEBUG_SERIAL.print("Present_Position(raw) : ");
DEBUG_SERIAL.println(i_present_position);
}
delay(500);
}
2021-04-05 10:58:09
willson
2021-04-05 19:47:57 Medha Shruti

Hi,

Thank you so much for the code. It worked. I think there was some problem with the coding part.

By the way, this was the video I had followed: https://www.youtube.com/watch?v=OvmGwDDrdu0

The hardware part worked fine but the code did not work.


Thanks a lot again :)

2021-04-05 19:47:57
medhas
2021-04-08 21:54:57 Medha Shruti

Hi,

Can you please guide me on how to configure the IDs?

Thanks

2021-04-08 21:54:57
medhas
2021-04-09 17:20:13 Will Son

Hi,

You can use DYNAMIXEL Wizard 2.0 to configure the DYNAMIXEL.

It is recommended to use U2D2 for managing DYNAMIXEL, but if you don't have one, you can use OpenCM9.04 after recovering the OpenCM9.04 firmware from R+Manager 2.0.

https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/#basic-features

Thanks!

2021-04-09 17:20:13
willson
2021-04-09 19:31:27 Medha Shruti

Hi, 

I tried to use the R+Manager and I am getting the following error. I don't have a U2D2. I am using the cable provided with the OpenCM9.04. Is there any tutorial I can follow? Can you send me the link to download R+Manager 2.0?



I tried another method of changing the ID. I used the Dynamixel2Arduino library and used the SetID function to change one of motor's ID. It worked.

Some of the questions I have are:

1. How do I control the motors simultaneously? With the above program I am able to move one motor at a time. 

2. How to control the speed of the motors?

3. The motors rotate in counterclockwise direction when I give the position in degree value. How do I make them turn in clockwise direction?


Thanks

2021-04-09 19:31:27
medhas
2021-04-11 02:35:37 Medha Shruti

Kindly suggest

2021-04-11 02:35:37
medhas
2021-04-12 13:08:14 Will Son

Hi,


Sorry about the confusion.

The OpenCM9.04 does not fully support AX or MX as it is originally designed for X series.

R+Manager 2.0 only supports product with Protocol 1.0 and does not support AX or MX series, therefore, please use DYNAMIXEL Wizard 2.0.

If you need to recover or update the firmware the AX and MX series, please use U2D2 and DYNAMIXEL Wizard 2.0


You can use DYNAMIXEL2Arduino examples in order to configure your AX or MX series.


1. You can add additional DYNAMIXEL to the basic example to run multiple DYNAMIXEL at the same time, but each DYNAMIXEL must have different IDs.

2. MX-12W and AX-12A has a data that controls Moving Speed(32). You can write to this address to control the speed. Please refer to eManual of each product for more details.

3. In case of AX-12A, when it is set to Joint Mode, it only operates between 0 ~ 300 degree. When set to Wheel Mode, you can write a 1024 ~ 2047 value to Moving Speed(32) to reverse rotate.


This can be done with DYNAMIXEL2Arduino, but it is much easier to test with U2D2 and DYNAMIXEL Wizard 2.0.

Thank you.

2021-04-12 13:08:14
willson
Reply
웹에디터 시작 웹 에디터 끝