Comments (17)
KurtE
commented on August 17, 2024
2
@Vali18 and (@OpusK @routiful @kijongGil )
I thought I would give it one more try, It is sort of kludge code, But it looks for values near 512. If it finds a value between 500-510 and another value 513-523 and no values < 500 or > 523 in between them than it assumes it completed a revolution... It appears to be working pretty well...
#include <DynamixelSDK.h>
/** EEPROM AREA **/
#define AX_MODEL_NUMBER_L 0
#define AX_MODEL_NUMBER_H 1
#define AX_VERSION 2
#define AX_ID 3
#define AX_BAUD_RATE 4
#define AX_RETURN_DELAY_TIME 5
#define AX_CW_ANGLE_LIMIT_L 6
#define AX_CW_ANGLE_LIMIT_H 7
#define AX_CCW_ANGLE_LIMIT_L 8
#define AX_CCW_ANGLE_LIMIT_H 9
#define AX_SYSTEM_DATA2 10
#define AX_LIMIT_TEMPERATURE 11
#define AX_DOWN_LIMIT_VOLTAGE 12
#define AX_UP_LIMIT_VOLTAGE 13
#define AX_MAX_TORQUE_L 14
#define AX_MAX_TORQUE_H 15
#define AX_RETURN_LEVEL 16
#define AX_ALARM_LED 17
#define AX_ALARM_SHUTDOWN 18
/** RAM AREA **/
#define AX_TORQUE_ENABLE 24
#define AX_LED 25
#define AX_CW_COMPLIANCE_MARGIN 26
#define AX_CCW_COMPLIANCE_MARGIN 27
#define AX_CW_COMPLIANCE_SLOPE 28
#define AX_CCW_COMPLIANCE_SLOPE 29
#define AX_GOAL_POSITION_L 30
#define AX_GOAL_POSITION_H 31
#define AX_GOAL_SPEED_L 32
#define AX_GOAL_SPEED_H 33
#define AX_TORQUE_LIMIT_L 34
#define AX_TORQUE_LIMIT_H 35
#define AX_PRESENT_POSITION_L 36
#define AX_PRESENT_POSITION_H 37
#define AX_PRESENT_SPEED_L 38
#define AX_PRESENT_SPEED_H 39
#define AX_PRESENT_LOAD_L 40
#define AX_PRESENT_LOAD_H 41
#define AX_PRESENT_VOLTAGE 42
#define AX_PRESENT_TEMPERATURE 43
#define AX_REGISTERED_INSTRUCTION 44
#define AX_PAUSE_TIME 45
#define AX_MOVING 46
#define AX_LOCK 47
#define AX_PUNCH_L 48
#define AX_PUNCH_H 49
// Protocol version
#define PROTOCOL_VERSION 1.0
// Default setting
#define DXL_ID 21 // Dynamixel ID: 1
#define BAUDRATE 1000000
#define DEVICENAME "3" // Check which port is being used on your controller
#define TORQUE_ENABLE 1 // Value for enabling the torque
#define TORQUE_DISABLE 0 // Value for disabling the torque
#define ESC_ASCII_VALUE 0x1b
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
// Communication result
int dxl_comm_result = COMM_TX_FAIL;
uint8_t dxl_error = 0;
uint16_t ax_present_position = 0;
uint16_t ax_previous_position = 0xffff;
uint16_t ax_moving_speed = 200;
bool position_output = false;
int revolution_count = 0;
bool value_between_500_510 = false;
bool value_between_513_523 = false;
uint32_t revolution_start_time;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
while (!Serial);
Serial.println("Start..");
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
// Open port
if (portHandler->openPort())
{
Serial.print("Succeeded to open the port!\n");
}
else
{
Serial.print("Failed to open the port!\n");
Serial.print("Press any key to terminate...\n");
return;
}
// Set port baudrate
if (portHandler->setBaudRate(BAUDRATE))
{
Serial.print("Succeeded to change the baudrate!\n");
}
else
{
Serial.print("Failed to change the baudrate!\n");
Serial.print("Press any key to terminate...\n");
return;
}
// Change Dynamixel CW Angle Limit
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_CW_ANGLE_LIMIT_L, 0, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.println(packetHandler->getRxPacketError(dxl_error));
}
else
{
Serial.println("Dynamixel CW angle set to 0 successfully!");
}
// Change Dynamixel CCW Angle Limit
Serial.println("Before AX_CCW_ANGLE_LIMIT_L"); Serial.flush();
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_CCW_ANGLE_LIMIT_L, 0, &dxl_error);
Serial.println("After AX_CCW_ANGLE_LIMIT_L"); Serial.flush();
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.println(packetHandler->getRxPacketError(dxl_error));
}
else
{
Serial.println("Dynamixel CCW angle set to 0 successfully!");
}
Serial.flush();
// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, AX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->getTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error);
}
else
{
Serial.print("Dynamixel has been successfully connected!\n");
}
// Write out startup speed
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_GOAL_SPEED_L, ax_moving_speed , &dxl_error);
PrintErrorStatus("AX_GOAL_SPEED_L", dxl_comm_result, dxl_error);
revolution_start_time = millis();
}
void loop() {
// Read present Position
dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, AX_PRESENT_POSITION_L, &ax_present_position , &dxl_error);
PrintErrorStatus("AX_PRESENT_POSITION_L", dxl_comm_result, dxl_error);
if (ax_present_position != ax_previous_position) {
if (position_output) {
Serial.printf("[ID:%03d] Pres:%d\n", DXL_ID, ax_present_position);
}
// Lets try looking around the half way mark of 512 as around 1024-0 transsition there is 60 degrees of
// not any valid values...
//
if ((ax_present_position >= 500) && (ax_present_position <= 510)) value_between_500_510 = true;
else if ((ax_present_position >= 513) && (ax_present_position <= 523)) value_between_513_523 = true;
else if ((ax_present_position < 500) || (ax_present_position > 523)) {
value_between_500_510 = false;
value_between_513_523 = false;
}
// if both values are set
if (value_between_500_510 && value_between_513_523) {
if (ax_moving_speed < 1024) revolution_count++;
else revolution_count--;
Serial.printf("Revl: %d Dt:%u\n", revolution_count, millis()-revolution_start_time);
revolution_start_time = millis();
value_between_500_510 = false;
value_between_513_523 = false;
}
ax_previous_position = ax_present_position;
}
if (Serial.available()) {
uint16_t new_goal_speed = 0;
int ch;
while (Serial.available()) {
ch = Serial.read();
if ((ch >= '0') && (ch <= '9')) {
new_goal_speed = new_goal_speed * 10 + ch - '0';
} else if ((ch == 'p') || (ch == 'P')) {
position_output = !position_output;
}
}
if (new_goal_speed != ax_moving_speed) {
// Write goal speed
ax_moving_speed = new_goal_speed;
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_GOAL_SPEED_L, ax_moving_speed , &dxl_error);
PrintErrorStatus("AX_GOAL_SPEED_L", dxl_comm_result, dxl_error);
if (ax_moving_speed < 1024) Serial.printf("New speed %d CCW\n", ax_moving_speed);
else Serial.printf("New speed %d CW\n", ax_moving_speed-1024);
}
}
}
void PrintErrorStatus(const char *psz, int dxl_comm_result, uint8_t dxl_error)
{
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.print(psz);
Serial.print(" ");
Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.print(psz);
Serial.print(" ");
Serial.println(packetHandler->getRxPacketError(dxl_error));
}
}
By default I don't show the actual read positions. You can enter a P or p to toggle showing the different values... You can also type in a decimal number for what speed you wish to go... The code now outputs some messages showing new speed. It also does some delta millis() for each revolution, so you can see if counters between revolutions looks valid.
Sample run output:
Start..
Succeeded to open the port!
Succeeded to change the baudrate!
Dynamixel CW angle set to 0 successfully!
Before AX_CCW_ANGLE_LIMIT_L
After AX_CCW_ANGLE_LIMIT_L
Dynamixel CCW angle set to 0 successfully!
Dynamixel has been successfully connected!
Revl: 1 Dt:4170
New speed 400 CCW
Revl: 2 Dt:2524
Revl: 3 Dt:2333
Revl: 4 Dt:2330
New speed 600 CCW
Revl: 5 Dt:2161
Revl: 6 Dt:1532
Revl: 7 Dt:1532
Revl: 8 Dt:1532
Revl: 9 Dt:1532
New speed 800 CCW
Revl: 10 Dt:1246
Revl: 11 Dt:1155
Revl: 12 Dt:1153
Revl: 13 Dt:1157
Revl: 14 Dt:1155
New speed 0 CCW
New speed 176 CW
Revl: 13 Dt:6093
Revl: 12 Dt:5540
New speed 376 CW
Revl: 11 Dt:5219
Revl: 10 Dt:2455
Revl: 9 Dt:2455
Revl: 8 Dt:2454
New speed 476 CW
Revl: 7 Dt:2433
Revl: 6 Dt:1921
Revl: 5 Dt:1924
New speed 0 CCW
I think I am pretty well done with it... Hopefully it works for you
from opencm9.04.
Vali18
commented on August 17, 2024
1
@KurtE This works perfectly!! I have no words to thank you :)!!!!!!!!!
from opencm9.04.
KurtE
commented on August 17, 2024
@Vali18, @ROBOTIS-Will and @OpusK:
Thought I would jump in here as I was mentioned and I have a working version of your code.
There are/were lots of issues. Some of which were mentioned on the forum.
Things like:
As @ROBOTIS-Will mentioned online you used the wrong registers, I missed that one when I caught the earlier errors like reading/writing 4 bytes instead of 2
Also your printf statements have issues, like:
Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
Should be:
Serial.printf("%s\n", packetHandler->getRxPacketError(dxl_error));
The \n is used to say print out a newline (i.e get to the next line on output)
You have the Torque Enable code commented out. Which I put back. Actually I moved it later in the code as with AX servos it is not important but if you move move to some of the newer servos you can not update areas in EEPROM when torque is enabled on the servo.
Probably the biggest issue I ran into was you defined global variables for the portHandler and packetHandler
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
And then in setup you had local versions of these variables:
and then locally in setup you defined (and set them)
ynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
dynamixel::PacketHandler *packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
So the global variables were never set, so when you reference them in loop(), you are indirecting off a NULL pointer and fault.
Here is a version of your program that is working for me. Note: I have a different SERVO ID...
#include <DynamixelSDK.h>
/** EEPROM AREA **/
#define AX_MODEL_NUMBER_L 0
#define AX_MODEL_NUMBER_H 1
#define AX_VERSION 2
#define AX_ID 3
#define AX_BAUD_RATE 4
#define AX_RETURN_DELAY_TIME 5
#define AX_CW_ANGLE_LIMIT_L 6
#define AX_CW_ANGLE_LIMIT_H 7
#define AX_CCW_ANGLE_LIMIT_L 8
#define AX_CCW_ANGLE_LIMIT_H 9
#define AX_SYSTEM_DATA2 10
#define AX_LIMIT_TEMPERATURE 11
#define AX_DOWN_LIMIT_VOLTAGE 12
#define AX_UP_LIMIT_VOLTAGE 13
#define AX_MAX_TORQUE_L 14
#define AX_MAX_TORQUE_H 15
#define AX_RETURN_LEVEL 16
#define AX_ALARM_LED 17
#define AX_ALARM_SHUTDOWN 18
/** RAM AREA **/
#define AX_TORQUE_ENABLE 24
#define AX_LED 25
#define AX_CW_COMPLIANCE_MARGIN 26
#define AX_CCW_COMPLIANCE_MARGIN 27
#define AX_CW_COMPLIANCE_SLOPE 28
#define AX_CCW_COMPLIANCE_SLOPE 29
#define AX_GOAL_POSITION_L 30
#define AX_GOAL_POSITION_H 31
#define AX_GOAL_SPEED_L 32
#define AX_GOAL_SPEED_H 33
#define AX_TORQUE_LIMIT_L 34
#define AX_TORQUE_LIMIT_H 35
#define AX_PRESENT_POSITION_L 36
#define AX_PRESENT_POSITION_H 37
#define AX_PRESENT_SPEED_L 38
#define AX_PRESENT_SPEED_H 39
#define AX_PRESENT_LOAD_L 40
#define AX_PRESENT_LOAD_H 41
#define AX_PRESENT_VOLTAGE 42
#define AX_PRESENT_TEMPERATURE 43
#define AX_REGISTERED_INSTRUCTION 44
#define AX_PAUSE_TIME 45
#define AX_MOVING 46
#define AX_LOCK 47
#define AX_PUNCH_L 48
#define AX_PUNCH_H 49
// Protocol version
#define PROTOCOL_VERSION 1.0
// Default setting
#define DXL_ID 21 // Dynamixel ID: 1
#define BAUDRATE 1000000
#define DEVICENAME "3" // Check which port is being used on your controller
#define TORQUE_ENABLE 1 // Value for enabling the torque
#define TORQUE_DISABLE 0 // Value for disabling the torque
#define ESC_ASCII_VALUE 0x1b
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
// Communication result
int dxl_comm_result = COMM_TX_FAIL;
uint8_t dxl_error = 0;
uint16_t ax_present_position = 0;
uint16_t ax_moving_speed = 1024 + 800;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
while (!Serial);
Serial.println("Start..");
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
// Open port
if (portHandler->openPort())
{
Serial.print("Succeeded to open the port!\n");
}
else
{
Serial.print("Failed to open the port!\n");
Serial.print("Press any key to terminate...\n");
return;
}
// Set port baudrate
if (portHandler->setBaudRate(BAUDRATE))
{
Serial.print("Succeeded to change the baudrate!\n");
}
else
{
Serial.print("Failed to change the baudrate!\n");
Serial.print("Press any key to terminate...\n");
return;
}
// Change Dynamixel CW Angle Limit
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_CW_ANGLE_LIMIT_L, 0, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.println(packetHandler->getRxPacketError(dxl_error));
}
else
{
Serial.println("Dynamixel CW angle set to 0 successfully!");
}
// Change Dynamixel CCW Angle Limit
Serial.println("Before AX_CCW_ANGLE_LIMIT_L"); Serial.flush();
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_CCW_ANGLE_LIMIT_L, 0, &dxl_error);
Serial.println("After AX_CCW_ANGLE_LIMIT_L"); Serial.flush();
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.println(packetHandler->getRxPacketError(dxl_error));
}
else
{
Serial.println("Dynamixel CCW angle set to 0 successfully!");
}
Serial.flush();
// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, AX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->getTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error);
}
else
{
Serial.print("Dynamixel has been successfully connected!\n");
}
}
void loop() {
// Write goal speed
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_GOAL_SPEED_L, ax_moving_speed , &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.printf("Goal %s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.printf("Goal %s\n", packetHandler->getRxPacketError(dxl_error));
}
// Read present Position
dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, AX_PRESENT_POSITION_L, &ax_present_position , &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.printf("Present %s\n", packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.printf("Present %s\n", packetHandler->getRxPacketError(dxl_error));
}
Serial.printf("[ID:%03d] Pres:%d\n", DXL_ID, ax_present_position);
delay(250);
}
Hope that helps
Kurt
from opencm9.04.
Vali18
commented on August 17, 2024
Thank you so much @KurtE this is awesome! I couldn't have done it without your support. Thank you!!!
I have tried the code and wheel mode works, still having issues reading...not sure why. But so far I am very happy that things are finally strarting to be clear. I am planning to spend more time on it, based on all the very useful info you guys gave me... I will let you know how goes !!
Thanks a lot and please feel free to send any examples or documentation across if you find any in the future :)
from opencm9.04.
KurtE
commented on August 17, 2024
Not sure what issues you are having reading. I was reading in data fine.
Note: The end of my loop had a delay of 250 or a quarter of a second. I put this in when I was debugging and it hung system out to dry. This is when I found that NULL pointer issue I mentioned.
You might remove it.
Also remember with the AX servos the values for position go 0-1091 and this only covers 300 degrees of position. That is what is nice with the newer servos like the XL430-w250 it has almost 4 times the resolution. The values go from 0-4095 and it covers 360 degrees. And about the same price as the older AX-12 servos
from opencm9.04.
Vali18
commented on August 17, 2024
Hi @KurtE
I tried different things (including removing the delay from your code) but the issue is that I can only read the initial position but the value does not update while the motor moves. I attach a screenshot of what is printed on the serial monitor
from opencm9.04.
KurtE
commented on August 17, 2024
Note It works for me, Yes you may get some duplicates in a row, but if you are asking that fast, then the may be likely.
There are obvious things I would do If it were me. You don't have to keep telling it to go at the same speed... i.e. you probably only need to tell it when you wish for it to change speeds. I would also probably not print everytime... Maybe instead only print when the value changes. Maybe keep a count of how many times it returned the same number in a row.
Also would try to reduce how much of the code is about printing failures... Maybe have sub-function which you pass the result and error code to and if necessary it prints them out...
Good luck
from opencm9.04.
KurtE
commented on August 17, 2024
@Vali18,
I forgot to mention, that again you should probably look at the E-Manuel for some of this:
Example if you look at the Present Position you will notice in the text a comment,
CAUTION : If it is set to Wheel Mode, the value cannot be used to measure the moving distance and the rotation frequency.
Which again if you move up to the XL servo might work better for you...
from opencm9.04.
Vali18
commented on August 17, 2024
Hi @KurtE yes I read this in the emanual...but when I used the workbench library I was able to read position values anyway, so I don't understand why not using SDK. I will try more things - like you suggested, reading only when a new value is read (?). I will definitely consider the XL for next stage of the project, but now we already have many AX12a...
Ultimately I need to count the number of rotations. My costum robot is moving around and we need to know where is in the physical space, so the number of rotation correspond to a position in the physical system. I need to add a plus one for every full rotation.
Is there any other way you would do it, if not reading position?
from opencm9.04.
KurtE
commented on August 17, 2024
You can read the values, but again the reliability may not be very high on the values especially if you are doing other things...
Here is yet another hacked up version:
#include <DynamixelSDK.h>
/** EEPROM AREA **/
#define AX_MODEL_NUMBER_L 0
#define AX_MODEL_NUMBER_H 1
#define AX_VERSION 2
#define AX_ID 3
#define AX_BAUD_RATE 4
#define AX_RETURN_DELAY_TIME 5
#define AX_CW_ANGLE_LIMIT_L 6
#define AX_CW_ANGLE_LIMIT_H 7
#define AX_CCW_ANGLE_LIMIT_L 8
#define AX_CCW_ANGLE_LIMIT_H 9
#define AX_SYSTEM_DATA2 10
#define AX_LIMIT_TEMPERATURE 11
#define AX_DOWN_LIMIT_VOLTAGE 12
#define AX_UP_LIMIT_VOLTAGE 13
#define AX_MAX_TORQUE_L 14
#define AX_MAX_TORQUE_H 15
#define AX_RETURN_LEVEL 16
#define AX_ALARM_LED 17
#define AX_ALARM_SHUTDOWN 18
/** RAM AREA **/
#define AX_TORQUE_ENABLE 24
#define AX_LED 25
#define AX_CW_COMPLIANCE_MARGIN 26
#define AX_CCW_COMPLIANCE_MARGIN 27
#define AX_CW_COMPLIANCE_SLOPE 28
#define AX_CCW_COMPLIANCE_SLOPE 29
#define AX_GOAL_POSITION_L 30
#define AX_GOAL_POSITION_H 31
#define AX_GOAL_SPEED_L 32
#define AX_GOAL_SPEED_H 33
#define AX_TORQUE_LIMIT_L 34
#define AX_TORQUE_LIMIT_H 35
#define AX_PRESENT_POSITION_L 36
#define AX_PRESENT_POSITION_H 37
#define AX_PRESENT_SPEED_L 38
#define AX_PRESENT_SPEED_H 39
#define AX_PRESENT_LOAD_L 40
#define AX_PRESENT_LOAD_H 41
#define AX_PRESENT_VOLTAGE 42
#define AX_PRESENT_TEMPERATURE 43
#define AX_REGISTERED_INSTRUCTION 44
#define AX_PAUSE_TIME 45
#define AX_MOVING 46
#define AX_LOCK 47
#define AX_PUNCH_L 48
#define AX_PUNCH_H 49
// Protocol version
#define PROTOCOL_VERSION 1.0
// Default setting
#define DXL_ID 21 // Dynamixel ID: 1
#define BAUDRATE 1000000
#define DEVICENAME "3" // Check which port is being used on your controller
#define TORQUE_ENABLE 1 // Value for enabling the torque
#define TORQUE_DISABLE 0 // Value for disabling the torque
#define ESC_ASCII_VALUE 0x1b
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
// Communication result
int dxl_comm_result = COMM_TX_FAIL;
uint8_t dxl_error = 0;
uint16_t ax_present_position = 0;
uint16_t ax_previous_position = 0xffff;
uint16_t ax_moving_speed = 200;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
while (!Serial);
Serial.println("Start..");
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
// Open port
if (portHandler->openPort())
{
Serial.print("Succeeded to open the port!\n");
}
else
{
Serial.print("Failed to open the port!\n");
Serial.print("Press any key to terminate...\n");
return;
}
// Set port baudrate
if (portHandler->setBaudRate(BAUDRATE))
{
Serial.print("Succeeded to change the baudrate!\n");
}
else
{
Serial.print("Failed to change the baudrate!\n");
Serial.print("Press any key to terminate...\n");
return;
}
// Change Dynamixel CW Angle Limit
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_CW_ANGLE_LIMIT_L, 0, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.println(packetHandler->getRxPacketError(dxl_error));
}
else
{
Serial.println("Dynamixel CW angle set to 0 successfully!");
}
// Change Dynamixel CCW Angle Limit
Serial.println("Before AX_CCW_ANGLE_LIMIT_L"); Serial.flush();
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_CCW_ANGLE_LIMIT_L, 0, &dxl_error);
Serial.println("After AX_CCW_ANGLE_LIMIT_L"); Serial.flush();
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.println(packetHandler->getRxPacketError(dxl_error));
}
else
{
Serial.println("Dynamixel CCW angle set to 0 successfully!");
}
Serial.flush();
// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, AX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->getTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error);
}
else
{
Serial.print("Dynamixel has been successfully connected!\n");
}
// Write out startup speed
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_GOAL_SPEED_L, ax_moving_speed , &dxl_error);
PrintErrorStatus("AX_GOAL_SPEED_L", dxl_comm_result, dxl_error);
}
void loop() {
if (Serial.available()) {
uint16_t new_goal_speed = 0;
int ch;
while (Serial.available()) {
ch = Serial.read();
if ((ch >= '0') && (ch <= '9')) {
new_goal_speed = new_goal_speed*10 + ch - '0';
}
}
if (new_goal_speed != ax_moving_speed) {
// Write goal speed
ax_moving_speed = new_goal_speed;
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_GOAL_SPEED_L, ax_moving_speed , &dxl_error);
PrintErrorStatus("AX_GOAL_SPEED_L", dxl_comm_result, dxl_error);
}
}
// Read present Position
dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, AX_PRESENT_POSITION_L, &ax_present_position , &dxl_error);
PrintErrorStatus("AX_PRESENT_POSITION_L", dxl_comm_result, dxl_error);
if (ax_present_position != ax_previous_position) {
Serial.printf("[ID:%03d] Pres:%d\n", DXL_ID, ax_present_position);
ax_previous_position = ax_present_position;
}
//delay(25);
}
void PrintErrorStatus(const char *psz, int dxl_comm_result, uint8_t dxl_error)
{
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.print(psz);
Serial.print(" ");
Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.print(psz);
Serial.print(" ");
Serial.println(packetHandler->getRxPacketError(dxl_error));
}
}
What is interesting in this version, is a quick and dirty check for Serial input at the start of Loop, where you can type in a decimal number for new speed, and then and only then does it update the ax_moving_speed. This is nice to allow you to see what different speeds do, both for actual speed stuff, but also to see how it changes direction.
It also only does outputs values if the values change.
It should not be hard to check for things like new value on one side of 0 and the old value on the other side to say you went a full rotation. Many of different ways to test this.
Again the more things (that can eat up a lot of time, like Serial.prints, can make accurate reading more difficult.
from opencm9.04.
KurtE
commented on August 17, 2024
I played around some more, and tried a first pass guess on calculating revolutions. Which does not fully work. Not sure if anyone wants to see the whole thing here...
#include <DynamixelSDK.h>
/** EEPROM AREA **/
#define AX_MODEL_NUMBER_L 0
#define AX_MODEL_NUMBER_H 1
#define AX_VERSION 2
#define AX_ID 3
#define AX_BAUD_RATE 4
#define AX_RETURN_DELAY_TIME 5
#define AX_CW_ANGLE_LIMIT_L 6
#define AX_CW_ANGLE_LIMIT_H 7
#define AX_CCW_ANGLE_LIMIT_L 8
#define AX_CCW_ANGLE_LIMIT_H 9
#define AX_SYSTEM_DATA2 10
#define AX_LIMIT_TEMPERATURE 11
#define AX_DOWN_LIMIT_VOLTAGE 12
#define AX_UP_LIMIT_VOLTAGE 13
#define AX_MAX_TORQUE_L 14
#define AX_MAX_TORQUE_H 15
#define AX_RETURN_LEVEL 16
#define AX_ALARM_LED 17
#define AX_ALARM_SHUTDOWN 18
/** RAM AREA **/
#define AX_TORQUE_ENABLE 24
#define AX_LED 25
#define AX_CW_COMPLIANCE_MARGIN 26
#define AX_CCW_COMPLIANCE_MARGIN 27
#define AX_CW_COMPLIANCE_SLOPE 28
#define AX_CCW_COMPLIANCE_SLOPE 29
#define AX_GOAL_POSITION_L 30
#define AX_GOAL_POSITION_H 31
#define AX_GOAL_SPEED_L 32
#define AX_GOAL_SPEED_H 33
#define AX_TORQUE_LIMIT_L 34
#define AX_TORQUE_LIMIT_H 35
#define AX_PRESENT_POSITION_L 36
#define AX_PRESENT_POSITION_H 37
#define AX_PRESENT_SPEED_L 38
#define AX_PRESENT_SPEED_H 39
#define AX_PRESENT_LOAD_L 40
#define AX_PRESENT_LOAD_H 41
#define AX_PRESENT_VOLTAGE 42
#define AX_PRESENT_TEMPERATURE 43
#define AX_REGISTERED_INSTRUCTION 44
#define AX_PAUSE_TIME 45
#define AX_MOVING 46
#define AX_LOCK 47
#define AX_PUNCH_L 48
#define AX_PUNCH_H 49
// Protocol version
#define PROTOCOL_VERSION 1.0
// Default setting
#define DXL_ID 21 // Dynamixel ID: 1
#define BAUDRATE 1000000
#define DEVICENAME "3" // Check which port is being used on your controller
#define TORQUE_ENABLE 1 // Value for enabling the torque
#define TORQUE_DISABLE 0 // Value for disabling the torque
#define ESC_ASCII_VALUE 0x1b
dynamixel::PortHandler *portHandler;
dynamixel::PacketHandler *packetHandler;
// Communication result
int dxl_comm_result = COMM_TX_FAIL;
uint8_t dxl_error = 0;
uint16_t ax_present_position = 0;
uint16_t ax_previous_position = 0xffff;
uint16_t ax_moving_speed = 200;
bool position_output = true;
int revolution_count = 0;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
while (!Serial);
Serial.println("Start..");
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);
// Initialize PacketHandler instance
// Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);
// Open port
if (portHandler->openPort())
{
Serial.print("Succeeded to open the port!\n");
}
else
{
Serial.print("Failed to open the port!\n");
Serial.print("Press any key to terminate...\n");
return;
}
// Set port baudrate
if (portHandler->setBaudRate(BAUDRATE))
{
Serial.print("Succeeded to change the baudrate!\n");
}
else
{
Serial.print("Failed to change the baudrate!\n");
Serial.print("Press any key to terminate...\n");
return;
}
// Change Dynamixel CW Angle Limit
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_CW_ANGLE_LIMIT_L, 0, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.println(packetHandler->getRxPacketError(dxl_error));
}
else
{
Serial.println("Dynamixel CW angle set to 0 successfully!");
}
// Change Dynamixel CCW Angle Limit
Serial.println("Before AX_CCW_ANGLE_LIMIT_L"); Serial.flush();
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_CCW_ANGLE_LIMIT_L, 0, &dxl_error);
Serial.println("After AX_CCW_ANGLE_LIMIT_L"); Serial.flush();
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.println(packetHandler->getRxPacketError(dxl_error));
}
else
{
Serial.println("Dynamixel CCW angle set to 0 successfully!");
}
Serial.flush();
// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, AX_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->getTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error);
}
else
{
Serial.print("Dynamixel has been successfully connected!\n");
}
// Write out startup speed
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_GOAL_SPEED_L, ax_moving_speed , &dxl_error);
PrintErrorStatus("AX_GOAL_SPEED_L", dxl_comm_result, dxl_error);
}
void loop() {
// Read present Position
dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, AX_PRESENT_POSITION_L, &ax_present_position , &dxl_error);
PrintErrorStatus("AX_PRESENT_POSITION_L", dxl_comm_result, dxl_error);
if (ax_present_position != ax_previous_position) {
if (position_output) {
Serial.printf("[ID:%03d] Pres:%d\n", DXL_ID, ax_present_position);
}
bool output_revolution = false;
uint16_t delta_position = abs(ax_present_position - ax_previous_position);
if ((ax_moving_speed < 1024) && (ax_present_position < ax_previous_position) && (delta_position > 10)) {
revolution_count++;
output_revolution = true;
}
else if ((ax_moving_speed >= 1024) && (ax_present_position > ax_previous_position) && (delta_position > 10)) {
revolution_count--;
output_revolution = true;
}
if (output_revolution) {
Serial.printf("Revl: %d\n", revolution_count);
}
ax_previous_position = ax_present_position;
}
if (Serial.available()) {
uint16_t new_goal_speed = 0;
int ch;
while (Serial.available()) {
ch = Serial.read();
if ((ch >= '0') && (ch <= '9')) {
new_goal_speed = new_goal_speed * 10 + ch - '0';
} else if ((ch == 'p') || (ch == 'P')) {
position_output = !position_output;
}
}
if (new_goal_speed != ax_moving_speed) {
// Write goal speed
ax_moving_speed = new_goal_speed;
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_GOAL_SPEED_L, ax_moving_speed , &dxl_error);
PrintErrorStatus("AX_GOAL_SPEED_L", dxl_comm_result, dxl_error);
}
}
}
void PrintErrorStatus(const char *psz, int dxl_comm_result, uint8_t dxl_error)
{
if (dxl_comm_result != COMM_SUCCESS)
{
Serial.print(psz);
Serial.print(" ");
Serial.println(packetHandler->getTxRxResult(dxl_comm_result));
}
else if (dxl_error != 0)
{
Serial.print(psz);
Serial.print(" ");
Serial.println(packetHandler->getRxPacketError(dxl_error));
}
}
But I put it here anyway. The Serial.read() code also looks for a p or P to be input and if so toggles on if to print the positions out or not. It also does a first pass at trying to guess, if we completed a revolution.
The key part of the code is:
bool output_revolution = false;
uint16_t delta_position = abs(ax_present_position - ax_previous_position);
if ((ax_moving_speed < 1024) && (ax_present_position < ax_previous_position) && (delta_position > 10)) {
revolution_count++;
output_revolution = true;
}
else if ((ax_moving_speed >= 1024) && (ax_present_position > ax_previous_position) && (delta_position > 10)) {
revolution_count--;
output_revolution = true;
}
if (output_revolution) {
Serial.printf("Revl: %d\n", revolution_count);
}
It looks at are we going clockwise or counter clockwise to know if the Position should be counting up or down... The first try was in one direction if we were up at near 1023 and then down to 0 (i.e looked like we wrapped around (new position < last position), than we completed a revolution.
First try was simply checking if the ax_present_position < ax_previous_position
But that was not sufficient as we get slop values so maybe counting goes like 80, 81, 80, 82, 83... So put in a slop test, which helps...
However there is still the issue that the encoder is only valid for 300 out of 360 degrees, so you end up with some garbage values in that area. Example output from run:
[ID:021] Pres:1019
[ID:021] Pres:1018
[ID:021] Pres:1019
[ID:021] Pres:1020
[ID:021] Pres:1021
[ID:021] Pres:1023
[ID:021] Pres:781
Revl: 2
[ID:021] Pres:699
Revl: 3
[ID:021] Pres:650
Revl: 4
[ID:021] Pres:634
Revl: 5
[ID:021] Pres:632
[ID:021] Pres:625
[ID:021] Pres:629
[ID:021] Pres:633
[ID:021] Pres:631
[ID:021] Pres:629
[ID:021] Pres:627
[ID:021] Pres:628
[ID:021] Pres:631
[ID:021] Pres:621
[ID:021] Pres:620
[ID:021] Pres:628
[ID:021] Pres:626
[ID:021] Pres:631
[ID:021] Pres:626
[ID:021] Pres:623
[ID:021] Pres:627
[ID:021] Pres:626
[ID:021] Pres:627
[ID:021] Pres:622
[ID:021] Pres:623
[ID:021] Pres:631
[ID:021] Pres:632
[ID:021] Pres:623
[ID:021] Pres:628
[ID:021] Pres:627
[ID:021] Pres:633
[ID:021] Pres:629
[ID:021] Pres:632
[ID:021] Pres:628
[ID:021] Pres:624
[ID:021] Pres:626
[ID:021] Pres:629
[ID:021] Pres:623
[ID:021] Pres:625
[ID:021] Pres:626
[ID:021] Pres:624
[ID:021] Pres:627
[ID:021] Pres:629
[ID:021] Pres:623
[ID:021] Pres:628
[ID:021] Pres:635
[ID:021] Pres:625
[ID:021] Pres:628
[ID:021] Pres:631
[ID:021] Pres:629
[ID:021] Pres:628
[ID:021] Pres:633
[ID:021] Pres:629
[ID:021] Pres:620
[ID:021] Pres:624
[ID:021] Pres:623
[ID:021] Pres:622
[ID:021] Pres:620
[ID:021] Pres:628
[ID:021] Pres:626
[ID:021] Pres:621
[ID:021] Pres:627
[ID:021] Pres:631
[ID:021] Pres:633
[ID:021] Pres:628
[ID:021] Pres:631
[ID:021] Pres:627
[ID:021] Pres:626
[ID:021] Pres:628
[ID:021] Pres:621
[ID:021] Pres:625
[ID:021] Pres:627
[ID:021] Pres:623
[ID:021] Pres:624
[ID:021] Pres:621
[ID:021] Pres:623
[ID:021] Pres:627
[ID:021] Pres:633
[ID:021] Pres:629
[ID:021] Pres:621
[ID:021] Pres:634
[ID:021] Pres:631
[ID:021] Pres:628
[ID:021] Pres:621
[ID:021] Pres:629
[ID:021] Pres:622
[ID:021] Pres:625
[ID:021] Pres:623
[ID:021] Pres:627
[ID:021] Pres:619
[ID:021] Pres:626
[ID:021] Pres:623
[ID:021] Pres:621
[ID:021] Pres:626
[ID:021] Pres:633
[ID:021] Pres:631
[ID:021] Pres:629
[ID:021] Pres:621
[ID:021] Pres:625
[ID:021] Pres:627
[ID:021] Pres:617
[ID:021] Pres:623
[ID:021] Pres:622
[ID:021] Pres:617
[ID:021] Pres:620
[ID:021] Pres:0
Revl: 6
[ID:021] Pres:1
[ID:021] Pres:2
[ID:021] Pres:3
[ID:021] Pres:4
[ID:021] Pres:5
[ID:021] Pres:6
[ID:021] Pres:7
[ID:021] Pres:9
Notice that it nicely counts up to 1023, than gets several garbage values before getting back to to counting 1, 2, 3, ...
So my above code does increment/decrement revolution counter, but several times during this transition... So you would still need to put in some other filtering, to know how a valid count...
Could be as simple as checking for values going up to some high value near 1023, and then ignore all values until you get one in the range of under some value like 10 or 20... Maybe verifying that it had a natural counting up before the break point and valid after break point and then increment your value...
Or if you experiment and find the garbage values are never in some range like 300-350, than you could look for counting that goes through that range to increment your revolution count...
Again I think you can probably get something that works pretty well. My guess is it will be more reliable at lower speeds, but...
from opencm9.04.
Vali18
commented on August 17, 2024
Dear @KurtE this is beyond impressive! Will have access to my motors again tomorrow and it will be the first thing I'll try!!! Will let you know... And again thanks so much for all your work and support!!
from opencm9.04.
KurtE
commented on August 17, 2024
Glad it works for you. My guess is this issue can probably be closed...
Not sure it would be worth converting something like the above code into a sample program that goes with the openCM IDE...
from opencm9.04.
Vali18
commented on August 17, 2024
@KurtE thanks a lot!
I think this is great idea!!
Would be great to have more sample codes like that!
@OpusK
from opencm9.04.
OpusK
commented on August 17, 2024
@KurtE
Thank you for your great contribution!
@Vali18, We will discuss with the person in charge of adding examples.
@kijongGil, What do you think about adding example above?
from opencm9.04.
KurtE
commented on August 17, 2024
Hard to say if this one warrants being included with IDE. Not sure if there is some other location that either code or simply approach might be mentioned on how to maybe support wheel mode on AX servos and get a revolution count...
from opencm9.04.
OpusK
commented on August 17, 2024
Okay, I will discuss internally with my team in this regard(include warranty issue).
I will end this issue :)
from opencm9.04.
Related Issues (20)
- br410 uart over ble profile code HOT 2
- Are they a set? OpenCM9.04 Release 1.5.0 and OpenCM9.04 Firmware ver 23. HOT 2
- OpenCM 9.04 model for circuit design programs HOT 6
- Will Wire.h be implemented in hardware? HOT 3
- How to modifying the PWM frequency on the OpenCM9.04 ? HOT 2
- Cant't make OpenCM communicate with sensors via I2C HOT 2
- stdint.h not found error on compile for OpenCM
- ros2 micro-ros support
- Arduino IDE Board Manager cannot install OpenCM board HOT 2
- Arduino IDE - Uploading error: exit status 0xc0000135 HOT 2
- Support all of the Arduino installs - Now including ARM64 HOT 1
- Serial Port problems with <ros.h> on arduino IDE HOT 5
- Conflict with wire.h HOT 1
- Issues about sync read and its examples HOT 2
- Problem with example "n_Bulk_read_write" (groupBulkWrite addparam failed) HOT 7
- [OpenCM904 Not Detected UART Flash Loader] HOT 3
- [3.7.21][C++][USB2Dynamixel][XM540-W150-R] Issue: Writing and reading negative value of the current HOT 1
- Bootloader source HOT 1
- openCM904C Serial/UART Not working HOT 8
- Can't connect to MCU with ST-LINK/V2 and ST-LINK Utility HOT 3
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from opencm9.04.