...
POST No. 2406928
DYNAMIXEL SDK ERROR - [TxRxResult] There is no status packet!
2018-08-10 03:39:24 vali18

Hello Everyone,


I  have been encountering problems in controlling dynamixel. 
I have multiple AX 12a and MX 64.

For my project I basically need to write speed, allow continous rotation and read current position, speed and load.
I have tried different hardware/connections setups, see pictures below.
I have uploadod the same code (see below) and got different issues.



I have several codes. I started using workbench library and I recently made a transition to sdk, hoping that was more stable. 

The code below worked yesterday fine and today is giving me this error.

Start..
Succeeded to open the port!
Succeeded to change the baudrate!
This function is deprecated. Use 'Serial.print()' and 'getRxPacketError()' instead



Press any key to continue! (or press q to quit!)


I am in the arduino IDE:



#include <DynamixelSDK.h>


// Control table address (AX-12A)
#define ADDR_DXL_CW_ANGLE_LIMIT          6
#define ADDR_DXL_CCW_ANGLE_LIMIT         8
#define ADDR_DXL_DRIVE_MODE             10
#define ADDR_PRO_TORQUE_ENABLE           24          
#define ADDR_MX_GOAL_POSITION           30
#define ADDR_DXL_GOAL_SPEED             32
#define ADDR_MX_PRESENT_POSITION        36
#define ADDR_DXL_PRESENT_SPEED          38

// Protocol version
#define PROTOCOL_VERSION                1.0                 // See which protocol version is used in the Dynamixel

// Default setting
#define DXL_ID                          1                  // Dynamixel ID: 1
#define BAUDRATE                        1000000
#define DEVICENAME                      "COM7"                 // Check which port is being used on your controller
                                                            // ex) Windows: "COM1"   Linux: "/dev/ttyUSB0"

#define TORQUE_ENABLE                   1                   // Value for enabling the torque
#define TORQUE_DISABLE                  0                   // Value for disabling the torque
//#define DXL_MINIMUM_POSITION_VALUE      100                 // Dynamixel will rotate between this value
#define DXL_MAXIMUM_POSITION_VALUE      300                // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
#define DXL_MOVING_STATUS_THRESHOLD     20                  // Dynamixel moving status threshold

#define ESC_ASCII_VALUE                 0x1b



void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  while(!Serial);
  Serial.println("Start..");


  // Initialize PortHandler instance
  // Set the port path
  // Get methods and members of PortHandlerLinux or PortHandlerWindows
  dynamixel::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);

  int index = 0;
  int dxl_comm_result = COMM_TX_FAIL;             // Communication result
  int dxl_goal_position = DXL_MAXIMUM_POSITION_VALUE;         // Goal position

  uint8_t dxl_error = 0;                          // Dynamixel error
  int32_t dxl_present_position = 0;               // Present position
  int32_t dxl_present_speed = 0;               // Present speed

  // 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;
  }

  // Enable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    packetHandler->printTxRxResult(dxl_comm_result);
  }
  else if (dxl_error != 0)
  {
    packetHandler->printRxPacketError(dxl_error);
  }
  else
  {
    Serial.print("Dynamixel has been successfully connected n");
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_DXL_CW_ANGLE_LIMIT, 0, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }
  else
  {
    printf("Dynamixel CW angle set to 0 successfully n");
  }

dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_DXL_CCW_ANGLE_LIMIT, 0, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }
  else
  {
    printf("Dynamixel CCW angle set to 0 successfully n");
  }


  while(1)
  {
    Serial.print("Press any key to continue! (or press q to quit!)n");


    while(Serial.available()==0);

    int ch;

    ch = Serial.read();
    if( ch == 'q' )
      break;

    // Write goal velocity
    dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_DXL_GOAL_SPEED, -800, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
      printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
      printf("%sn", packetHandler->getRxPacketError(dxl_error));
    }

do
    {
      // Read present velocity
      dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL_ID, ADDR_DXL_PRESENT_SPEED, (uint32_t*)&dxl_present_speed, &dxl_error);
      if (dxl_comm_result != COMM_SUCCESS)
      {
        printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
      }
      else if (dxl_error != 0)
      {
        printf("%sn", packetHandler->getRxPacketError(dxl_error));
      }

      printf("[ID:%03d] Goal:%f  Pres:%fn", DXL_ID, 500, dxl_present_speed);

    }
    
    while(10 > 0.5);
   
}

}

void loop() {
  // put your main code here, to run repeatedly:

}
 
2018-08-10 03:39:24
valentina
2018-08-10 05:41:18 Kurt

Again probably @Will Son - may have more of an idea of the question?



Again I am assuming you are trying to build for Arduino IDE for OpenCM 9.04 board.


But if your question is that this program fails... My guessing would be:

// Default setting
#define DXL_ID                          1                  // Dynamixel ID: 1
#define BAUDRATE                        1000000
#define DEVICENAME                      "COM7"                 // Check which port is being used on your controller
                                                            // ex) Windows: "COM1"   Linux: "/dev/ttyUSB0"

#define TORQUE_ENABLE                   1                   // Value for enabling the torque
#define TORQUE_DISABLE                  0                   // Value for disabling the torque
//#define DXL_MINIMUM_POSITION_VALUE      100                 // Dynamixel will rotate between this value
#define DXL_MAXIMUM_POSITION_VALUE      300                // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
#define DXL_MOVING_STATUS_THRESHOLD     20                  // Dynamixel moving status threshold

#define ESC_ASCII_VALUE                 0x1b



void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  while(!Serial);
  Serial.println("Start..");


  // Initialize PortHandler instance
  // Set the port path
  // Get methods and members of PortHandlerLinux or PortHandlerWindows
  dynamixel::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);

  int index = 0;
  int dxl_comm_result = COMM_TX_FAIL;             // Communication result
  int dxl_goal_position = DXL_MAXIMUM_POSITION_VALUE;         // Goal position

  uint8_t dxl_error = 0;                          // Dynamixel error
  int32_t dxl_present_position = 0;               // Present position
  int32_t dxl_present_speed = 0;               // Present speed

  // Open port
  if (portHandler->openPort())
  {
    Serial.print("Succeeded to open the port!n");
  }
 

IN the Above Code for Arduino the Device name (I show in color) is probably wrong.  That is for a Windows system setup.


For Arduino for OpenCM9.04, in your top picture, where you are using OpenCM9.04 ports directly it should be: #define DEVICENAME                      "1"

Which implies Serial1 on the board.


For the 2nd picture where you are using an OpenCM485 expansion board.  It should be: #define DEVICENAME                      "3"

As on the 485 Expansion board the servos are on Serial3 of the OpenCM9.04 board.





2018-08-10 05:41:18
Kurt
2018-08-10 09:08:13 valentina

Hi Kurt,


thanks for your reply. Unfortunately I tried all the combinations, changing the 

 #define DEVICENAME  to 3 and to 1 still doesen't work and print this error on the serial port:


Start..

Succeeded to open the port!

Succeeded to change the baudrate!

This function is deprecated. Use 'Serial.print()' and 'getRxPacketError()' instead

[TxRxResult] There is no status packet!

Press any key to continue! (or press q to quit!)



2018-08-10 09:08:13
valentina
2018-08-10 09:10:44 Will Son

Hello,


As Kurt mentioned above, the DEVICENAME has to be if you connected Dynamixel to OpenCM485 EXP board.

I've updated the comment in the latest OpenCM9.04 board manager release(1.2.0).


"This function is deprecated. Use 'Serial.print()' and 'getRxPacketError()' instead" is not an error but a warning that notifies to use get***() functions instead of print***() functions. 

Even with the warning, you can keep running the code by pressing any key.

You can also get rid of this warning message by changing print***() to get***() in the below highlighted lines.


// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->printTxRxResult(dxl_comm_result); // use getTxRxResult
}
else if (dxl_error != 0)
{
packetHandler->printRxPacketError(dxl_error); // use getRxPacketError
}
else
{
Serial.print("Dynamixel has been successfully connected \n");
}


2018-08-10 09:10:44
Will Son
2018-08-11 02:46:48 valentina

Hi Will Son and kurt, thank you both. I made the change, but the motors still don't work. 


It reads values: goal position and present position but it doesn't do anything! Seems that only reads :( 


Any idea pls?


2018-08-11 02:46:48
valentina
2018-08-11 08:30:24 Kurt

Maybe it would help to have more complete information, like current program you are running?  Is it the above code with the DEVICENAME set? 


What hardware are you running?  The top one talking to something I am not sure of, maybe PRO?


Or the bottom one with the AX servo? Connected to 485 board which would be DEVICENAME="3"


Again if talking to PRO, I would guess it would use Protocol=2.0  where the AX is Protocol= 1.0.


What ID does your servo have?  It looks like the AX servo has a sticker implying ID=3, where the original code showed it wanting to talk to DXL_ID 1


Looks like you are setting the Torque enable to 1  (#define ADDR_PRO_TORQUE_ENABLE           24) 


The other problem you have is on the line:

dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_DXL_GOAL_SPEED, -800, &dxl_error);

If you set the goal position to 800 it will run.  But -800 is not valid.

If you look at the definition: http://emanual.robotis.com/docs/en/dxl/ax/ax-12a/#moving-speed


I actually like the old AX-12 PDF file description:  Which does not cut and paste here very well, but

bits 15-11: need to be 0

bit 10: 0 CCW and 1 CW dirction

bits 0-9: speed value (0-1023)


So the -800 sets bits 11-15 as 1 and probably fails.  To get that speed you could probably do something like 800+1024.


If it still does not work another thing to check is that you have the servo power turned on.... That is the program can load and run with the USB power, but  the servos will only do something if the power is plugged into the port on the 485 board AND the power switch (black toggle) is turned on.  White dot down.


After I tried all of the above, the program worked...


Except I fixed all of the print statements.  That is a lot of the later ones just do printf, when they should have done Serial.printf.

Also maybe a cut/paste problem, but line terminations were wrong.  That is most of the printf statements had:

....n"  and it should instead have ...n"  Hope that makes sense.


Hope it works for you.

comment
2018-08-13 09:16:53 Will Son
Somehow this board doesn't properly shows backslash in the code.
Using double backslash helped me to display a single backslash in the code so I modified my reply.
Thank you for letting me know :)
2018-08-13 09:16:53
Will Son
2018-08-11 08:30:24
Kurt
2018-08-12 01:54:07 Kurt

Forgot to mention, I am using a nice new shiny openCM9.04 and 485 expansion board, that I assembled yesterday, (almost stock), I used extra long breakout pins on the OpenCM9.04, so I could easily have access to all of the IO pins from the top...



Here is the modified version of the above, that worked:

#include <DynamixelSDK.h>


// Control table address (AX-12A)
#define ADDR_DXL_CW_ANGLE_LIMIT          6
#define ADDR_DXL_CCW_ANGLE_LIMIT         8
#define ADDR_DXL_DRIVE_MODE             10
#define ADDR_PRO_TORQUE_ENABLE           24
#define ADDR_MX_GOAL_POSITION           30
#define ADDR_DXL_GOAL_SPEED             32
#define ADDR_MX_PRESENT_POSITION        36
#define ADDR_DXL_PRESENT_SPEED          38

// Protocol version
#define PROTOCOL_VERSION                1.0                 // See which protocol version is used in the Dynamixel

// Default setting
#define DXL_ID                          21                // Dynamixel ID: 1
#define BAUDRATE                        1000000
#define DEVICENAME                      "3"                 // Check which port is being used on your controller
// ex) Windows: "COM1"   Linux: "/dev/ttyUSB0"

#define TORQUE_ENABLE                   1                   // Value for enabling the torque
#define TORQUE_DISABLE                  0                   // Value for disabling the torque
//#define DXL_MINIMUM_POSITION_VALUE      100                 // Dynamixel will rotate between this value
#define DXL_MAXIMUM_POSITION_VALUE      300                // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
#define DXL_MOVING_STATUS_THRESHOLD     20                  // Dynamixel moving status threshold

#define ESC_ASCII_VALUE                 0x1b



void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  while (!Serial);
  Serial.println("Start..");


  // Initialize PortHandler instance
  // Set the port path
  // Get methods and members of PortHandlerLinux or PortHandlerWindows
  dynamixel::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);

  int index = 0;
  int dxl_comm_result = COMM_TX_FAIL;             // Communication result
  int dxl_goal_position = DXL_MAXIMUM_POSITION_VALUE;         // Goal position

  uint8_t dxl_error = 0;                          // Dynamixel error
  int32_t dxl_present_position = 0;               // Present position
  int32_t dxl_present_speed = 0;               // Present speed

  // 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;
  }

  // Enable Dynamixel Torque
  dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    packetHandler->printTxRxResult(dxl_comm_result);
  }
  else if (dxl_error != 0)
  {
    packetHandler->printRxPacketError(dxl_error);
  }
  else
  {
    Serial.print("Dynamixel has been successfully connected n");
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_DXL_CW_ANGLE_LIMIT, 0, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }
  else
  {
    Serial.printf("Dynamixel CW angle set to 0 successfully n");
  }

  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_DXL_CCW_ANGLE_LIMIT, 0, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }
  else
  {
    Serial.printf("Dynamixel CCW angle set to 0 successfully n");
  }


  while (1)
  {
    Serial.print("Press any key to continue! (or press q to quit!)n");


    while (Serial.available() == 0);

    int ch;

    ch = Serial.read();
    if ( ch == 'q' )
      break;

    // Write goal velocity
    dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_DXL_GOAL_SPEED, 800 + 1024, &dxl_error);
    if (dxl_comm_result != COMM_SUCCESS)
    {
      Serial.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
    }
    else if (dxl_error != 0)
    {
      Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
    }

    do
    {
      // Read present velocity
      dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL_ID, ADDR_DXL_PRESENT_SPEED, (uint32_t*)&dxl_present_speed, &dxl_error);
      if (dxl_comm_result != COMM_SUCCESS)
      {
        Serial.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
      }
      else if (dxl_error != 0)
      {
        Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
      }

      Serial.printf("[ID:%03d] Goal:%f  Pres:%fn", DXL_ID, 500, dxl_present_speed);

    }

    while (10 > 0.5);

  }

}

void loop() {
  // put your main code here, to run repeatedly:

}

 And not sure if you would find useful, but I first used my OpenCM_Find_Servos program, to figure out what Servo ID that servo had which turned out to be #21, It also caught me forgetting to turn on the Servo power (toggle switch...)


The test program I mentioned is part of one of my github projects.  Sources can be found at: https://github.com/KurtE/Open_CM904/blob/WIP_Support_XL320_Servos/OpenCM_Find_Servos/OpenCM_Find_Servos.ino


It scans both the Serail1 and Serial3 devices for both Protocol 1 and Protocol 2 devices and tells you which ones exist...

2018-08-12 01:54:07
Kurt
2018-08-12 07:21:02 valentina

Hi Kurt,


thank you so much for your reply.Your help is very much appreciated!!

I run your program, which was super helpful. It made me realise there is a problem with the connection between my open CM9.04 and the 485 extention board, which i somehow solved I hope! I will need to write robotis about it, since its a brand new board and doesn't work witouth the extra connection (but this is a minor issue at the moment :)!).

So now the basic weel mode CW rotation code works again, thanks to your help!!

I still have a couple of doubts and things that prevent me to move forward....

To answer to your comment " Looks like you are setting the Torque enable to 1 " 
isn't correct? 

And also you mentioned 

that I have a  problem on this line: 

dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_DXL_GOAL_SPEED, -800, &dxl_error);

Here is were i was trying to make continous rotation on the other direction (ccw). In workbench I simply had to write speed 300 and -300 to make it rotate on the opposite direction. How does it work here?

I see in your code is 
                                          
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_DXL_GOAL_SPEED, 800 + 1024, &dxl_error);  

Is not fully clear to me why is 800 + 1024  and also what value would you write to move it on the other direction?


Last thing - which is the main issue at the moment...I can't print values.

Serial.printf("[ID:%03d] Goal:%f  Pres:%fn n", DXL_ID, dxl_present_position, dxl_present_speed);  

this is what I get when the code runs:



2018-08-12 07:21:02
valentina
2018-08-12 07:21:36 valentina


 

2018-08-12 07:21:36
valentina
2018-08-12 07:28:35 valentina

current setup









 

2018-08-12 07:28:35
valentina
2018-08-12 23:34:36 Kurt

Hi again.


Hopefully @will son - will fill in more details that I am probably missing here.   But I am guessing it is maybe 10 or 11 PM Sunday night there right now... So it may be a few hours...


With setting the moving speed, look at the e-manual for the speed in wheel mode: http://emanual.robotis.com/docs/en/dxl/ax/ax-12a/#moving-speed

0 ~ 2,047(0x7FF) can be used, the unit is about 0.1%.
If a value in the range of 0 ~ 1,023 is used, it is stopped by setting to 0 while rotating to CCW direction.
If a value in the range of 1,024 ~ 2,047 is used, it is stopped by setting to 1,024 while rotating to CW direction.
That is, the 10th bit becomes the direction bit to control the direction.
In Wheel Mode, only the output control is possible, not speed.
For example, if it is set to 512, it means the output is controlled by 50% of the maximum output.

So the valid values are 0-2047 where 1024 is the center point (0 speed in the CW direction. 

Now my guess is passing in -800 will fail as I mentioned as the high bits are on which must be 0...


As for your wiring?  Not sure what your spliced cable is doing between the Servo connectors on the 904 board and the 485 board.


Again the connectors on the 904B? board  should be serviced by Serial1 (passing "1"  as DEVICENAME) in:

ynamixel::PortHandler *portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);


The Servo connections on the 485 board are serviced by Serial3  (again DEVICENAME "3")  Which I have in the program I pasted above.  


So the signal wire of that cable is connecting the outputs of Serial1 and Serial3 to each other?  Which could hamper things.  Actually it is connected to TTL line near the D,X,L pins


Now if you were trying to run the Servos on the Connectors of the 904 board, by default they will not get power from the power connector of the 485 board, which allows you to connect up different power for these.  Example you may want to run some XL320 servos at 7.2v and your other servos at the 11-12v (or more) on the 485 board.   There is a jumper on the 485 board as shown in the E-manual:  http://emanual.robotis.com/docs/en/parts/controller/opencm485exp/  look at the Power connection section for details about this jumper.


Whenever I run into issues where it sometimes works or not, one of the first things I would check is my solder connections. 

For example For the Serial3 stuff to work on 485 board, the connections for Serial3 (Pins 24, 25) must work, plus the pin it uses to set the direction (Read or Write), which is Arduino pin 22.

So I would double check these.  Make sure that they  are not  cold joint or not any shorts...

 

2018-08-12 23:34:36
Kurt
2018-08-13 09:22:58 Will Son

Hello Valentina,


As you've been working with Kurt during weekend, I think you're getting closer to the right direction.

However, what I don't understand is the connection between OpenCM9.04 board and OpenCM 485 EXP board.

They do not require additional cabling because they are connected with header pins.

This might cause signal interference, so please try without the cable.


Also, you need to turn on the OpenCM 485 EXP board in order to supply power to connected Dynamixel.

Please turn on the switch(ON is marked with a white dot) and check Dynamixel LED blinks once when powered.


Thank you.

2018-08-13 09:22:58
Will Son
2018-08-13 22:40:21 Kurt

Hello again @valentina  and @Will Son -


Actually it looks like we all missed looking for the first obvious issue!  


Look at the picture of my setup in a posting a few up from here.   Actually here it is again:


 


Then look carefully at yours. 

Also look at the instructions: http://emanual.robotis.com/docs/en/parts/controller/opencm485exp/#connecting-opencm904


Notice where the USB plug in as compared to the to things like the power plug in on the 485 board.  You will notice it is on the other end!


You installed the OpenCM9.04 backwards in the OpenCM 485 expansion board!

2018-08-13 22:40:21
Kurt
2018-08-14 05:44:50 valentina

Hi @kurt and @will,


thank you so much for your help!!

The motor is up and running, so that's great. Last step would be to read values...Still don't manage to get them correctly.

it prints weird numbers...does any of you have idea how can i get right values?

I would need present load and present position...
2018-08-14 05:44:50
valentina
2018-08-14 08:42:06 Kurt

Good to hear you are making progress.  Printing weird numbers does not give me any idea of what the current issue is.


Might help to see the actual current program and what output you are expecting and see the actual  output to better understand. 

2018-08-14 08:42:06
Kurt
2018-08-14 09:37:54 Will Son

Oh my god, I totally missed the orientation!! Very good call, Kurt.

It is very lucky of you Valentina that your OpenCM9.04 is still alive because most cases it ends up burning some chips and had to be repaired.

Regarding weird numbers.. not sure if this will help but please try using decimals instead of floating.

Serial.printf("[ID:%03d] Goal:%d  Pres:%d\n\n", DXL_ID, dxl_present_position, dxl_present_speed); 

2018-08-14 09:37:54
Will Son
2018-08-15 04:25:54 valentina

Hi both,


thanks and I am glad nothing happened to the board!! Need to be more careful for sure...but we had to try so many things to make them work and we (my team) find hard to use them sometimes - as we are beginners comparing to all of you!


That is why we write all the time :)!! But we feel we are getting slowly better - so we are very excited to see how dynamixel are contributing to the development our project!!


Anyway, @Will, thanks for the tip, I replaced the print line with the one above, but unfortunately I stil can't get right values...


Here the screen shot of what I see on my serial monitor:


2018-08-15 04:25:54
valentina
2018-08-15 04:26:44 valentina


 

2018-08-15 04:26:44
valentina
2018-08-15 09:20:59 Kurt

Where are you getting the values to print out, that are in the line: Serial.printf("[ID:%03d] Goal:%d  Pres:%dnn", DXL_ID, dxl_present_position, dxl_present_speed);

That is where is dxl_present_position and dxl_present_speed set?


That is the last version of the code I saw, only defined: int32_t  dxl_present_position = 0;

And never used it.


As for dxl_present_speed you define it:

  int32_t dxl_present_speed = 0;               // Present speed
 

You read it in with the line:

      dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, DXL_ID, ADDR_DXL_PRESENT_SPEED, (uint32_t*)&dxl_present_speed, &dxl_error);


The problem here is you are reading 4 bytes in and on AX servos present speed (and position) are two bytes long.


You need to change both variables to be something like: uint16_t present_speed;


And you need to change the read to be:

      dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, ADDR_DXL_PRESENT_SPEED, (uint32_t*)&dxl_present_speed, &dxl_error);


Or you can try to get fancy and try to read in 4 bytes at the address for PRESENT_POSITION and know that the following 2 bytes are PRESENT_SPEED...


2018-08-15 09:20:59
Kurt
2018-08-16 05:50:08 valentina

Thanks make sense, just got confused as in the code example they are showing write4byte method to write pos - but might be for other motors (?).


Generally, is there some link / clear documentation and/or example I can refer to? It is very hard to guess everytime what is the right sintax....

Shall i refer to the table here?

http://emanual.robotis.com/docs/en/dxl/ax/ax-12a/ 


Since I tried so many things and got quite confused I am now trying to re-write a super simple basic code that I can use as a base. I would like to write speed (wheel mode) and read position of one dynamixel.

The only arduino example I found is quite different from what I am used to. The code is written on the setup() using the while(1). I would like to use the standard arduino structure:
Setup() - code to run once 
Loop() - main code to run repeatedly.

Side note to give a little of context... my goal is to re-write a code I had few weeks ago using workbench and replacing it with dynamixel sdk. I had two motors and I could control the motor writing a speed and reading the position. So for example if I wrote 2800 on the serial motor, motor ID 2 was moving at 800 speed. 
The issue I had was that the workbench code gave me a lot of troubles uploading on OpenCM904. From what I see, the code written with Dynamixel SDK does not give problem uploading, but is hard to manipulate. Again, given the fact that we are no pros and we have limited time to finish this project...


Here's one of the codes I started (that does not work :( ....)


#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
#define AX_OPERATING_MODE           19
#define AX_DOWN_CALIBRATION_L       20
#define AX_DOWN_CALIBRATION_H       21
#define AX_UP_CALIBRATION_L         22
#define AX_UP_CALIBRATION_H         23
/** 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                          1                // 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;                     
int16_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..");

  dynamixel::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);

  // 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;
  }
/*
  // 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");
  }
 */ 
  // 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.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }
  else
  {
    Serial.printf("Dynamixel CW angle set to 0 successfully!n");
  }
  
  // Change Dynamixel CCW Angle Limit
  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_CCW_ANGLE_LIMIT_L, 0, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }
  else
  {
    Serial.printf("Dynamixel CCW angle set to 0 successfully!n");
  }

}

void loop() {   
  // Write goal speed
  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_GOAL_SPEED_H, ax_moving_speed , &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }

  // Read present Position
  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_PRESENT_POSITION_H, ax_present_position , &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }

  Serial.printf("[ID:%03d] Pres:%dnn", DXL_ID, ax_present_position); 
}


2018-08-16 05:50:08
valentina
2018-08-16 05:56:05 valentina
REVISED  VERSION FOR READING (STILL NOT WORKING):


#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
#define AX_OPERATING_MODE           19
#define AX_DOWN_CALIBRATION_L       20
#define AX_DOWN_CALIBRATION_H       21
#define AX_UP_CALIBRATION_L         22
#define AX_UP_CALIBRATION_H         23
/** 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                          1                // 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;                     
int16_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..");

  dynamixel::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);

  // 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;
  }
/*
  // 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");
  }
 */ 
  // 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.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }
  else
  {
    Serial.printf("Dynamixel CW angle set to 0 successfully!n");
  }
  
  // Change Dynamixel CCW Angle Limit
  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_CCW_ANGLE_LIMIT_L, 0, &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }
  else
  {
    Serial.printf("Dynamixel CCW angle set to 0 successfully!n");
  }

}

void loop() {   
  // Write goal speed
  dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_GOAL_SPEED_H, ax_moving_speed , &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }

  // Read present Position
  dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, AX_PRESENT_POSITION_H, ax_present_position , &dxl_error);
  if (dxl_comm_result != COMM_SUCCESS)
  {
    Serial.printf("%sn", packetHandler->getTxRxResult(dxl_comm_result));
  }
  else if (dxl_error != 0)
  {
    Serial.printf("%sn", packetHandler->getRxPacketError(dxl_error));
  }

  Serial.printf("[ID:%03d] Pres:%dnn", DXL_ID, ax_present_position); 
}
 
2018-08-16 05:56:05
valentina
2018-08-16 06:22:19 Kurt

I down loaded your current code (last post) and tried compiling and it does not compile. 


The line:

 dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, AX_PRESENT_POSITION_H, ax_present_position , &dxl_error);

 Gives the error:

yyy:176: error: invalid conversion from 'int16_t {aka short int}' to 'uint16_t* {aka short unsigned int*}' [-fpermissive]


invalid conversion from 'int16_t {aka short int}' to 'uint16_t* {aka short unsigned int*}' [-fpermissive]

 Couple of problems.  First off the values are unsigned so it is wanting ax_present_position to be defined as uint16_t


The second issue is you need to pass this variable by address not value.  That is you need to allow the function you are calling to fill in the value.  So the line should be:

dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, DXL_ID, AX_PRESENT_POSITION_H, &ax_present_position , &dxl_error);


2018-08-16 06:22:19
Kurt
2018-08-16 07:26:51 valentina

great Kurt thanks a lot..didn't have idea of this!! Whats about the writing function? The motor still doesn't move...

2018-08-16 07:26:51
valentina
2018-08-16 10:38:36 Will Son

Hi Valentina,


I found (almost) perfect example for you.

The following example will iterate your AX-series Dynamixel between 0 and 1,000 position while printing present position.

I'm sorry for not being able to spend more time to debug your code.


But, what I found wrong is the address you are using. You have to use Low address for writing and reading values as all multi byte data is following Little Endian.

So, in the below code, what you have to do is replace AX_GOAL_SPEED_H to AX_GOAL_SPEED_L.

dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_GOAL_SPEED_L, ax_moving_speed , &dxl_error); 


This also applies to the reading instruction and as it reads from H byte and the following byte that is not related to the present position, your value should be pretty much useless.

dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, AX_PRESENT_POSITION_L, ax_present_position , &dxl_error); 


Hopefully below example will solve almost all issues you currently have.


#include <DynamixelSDK.h>

#define ADDR_PRO_TORQUE_ENABLE 24 // Control table address is different in Dynamixel model
#define ADDR_PRO_GOAL_POSITION 30
#define ADDR_PRO_PRESENT_POSITION 36
#define MOVING 46

// Protocol version
#define PROTOCOL_VERSION 1.0 // See which protocol version is used in the Dynamixel

// Default setting
#define DXL_ID 1 // Dynamixel ID: 1
#define BAUDRATE 1000000
#define DEVICENAME "3" // Select Open485 EXP Board Port

#define TORQUE_ENABLE 1 // Value for enabling the torque
#define TORQUE_DISABLE 0 // Value for disabling the torque
#define DXL_MINIMUM_POSITION_VALUE 0 // Dynamixel will rotate between this value
#define DXL_MAXIMUM_POSITION_VALUE 1000 // and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
#define DXL_MOVING_STATUS_THRESHOLD 20 // Dynamixel moving status threshold

#define ESC_ASCII_VALUE 0x1b

// Create PortHandler instance
dynamixel::PortHandler *portHandler;

// Create PacketHandler instance
dynamixel::PacketHandler *packetHandler;

//***********Set Global Variables****************
int goalPosition = 0;
int isMoving = 0;
int dxl_comm_result = COMM_TX_FAIL; // Communication result
uint8_t dxl_error = 0; // Dynamixel error
int16_t dxl_present_position = 0; // Present position

void setup() {
// put your setup code here, to run once:
//Serial.attachInterrupt(usbInterrupt);
Serial.begin(115200);
while(!Serial);

// Initialize portHandler. Set the port path
// Get methods and members of PortHandlerLinux or PortHandlerWindows
portHandler = dynamixel::PortHandler::getPortHandler(DEVICENAME);

// Initialize packetHandler. Set the protocol version
// Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
packetHandler= dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION);

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;
}

// Enable Dynamixel Torque
dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
if (dxl_comm_result != COMM_SUCCESS)
{
packetHandler->getTxRxResult(dxl_comm_result);
}
else if (dxl_error != 0)
{
packetHandler->getRxPacketError(dxl_error);
}
else
{
Serial.print("Dynamixel has been successfully connected \n");
}
}

void loop() {
// put your main code here, to run repeatedly:
packetHandler->read1ByteTxRx(portHandler, DXL_ID, MOVING, (uint8_t*)&isMoving, &dxl_error);

if( isMoving == 0 ){ //if ID 1 dynamixel is stopped
//Send instruction packet to move for goalPosition( control address is 30 )
//Compatible with all dynamixel serise
dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_GOAL_POSITION, goalPosition, &dxl_error);
//toggle the position if goalPosition is 1000, set to 0, if 0, set to 1000
if(goalPosition == 1000)
goalPosition = 0;
else
goalPosition = 1000;
}
packetHandler->read2ByteTxRx(portHandler, DXL_ID, ADDR_PRO_PRESENT_POSITION, (uint16_t*)&dxl_present_position, &dxl_error);
Serial.print("Present Position : ");
Serial.print(dxl_present_position);
Serial.print("\n");
}
2018-08-16 10:38:36
Will Son
2018-08-17 00:09:13 Kurt

Thought I would mention I have a working version of your code.  There were several issues including the ones mentioned earlier by myself and @Will Son,


Probably the biggest issue was you have global variables for packetHandler and portHandler which were never set so when you use them in loop() the program faults.  You had local variables with the same name within setup().  Needed to remove the variable declarations there and instead just do the asignment...


More details including the version of code that ran on my OpenCM9.04/485 boards is up on the issue you created on github:

https://github.com/ROBOTIS-GIT/OpenCM9.04/issues/31


Hope that helps

2018-08-17 00:09:13
Kurt
2018-08-17 05:37:13 valentina

Thank you both very much!!! This is awesome


I have tried Kurt code and wheel mode works fine (still some issues with reading). The one @will sent is reading, but at the moment does not move...

But so far I am very happy that things are finally starting to become clear and I am planning to work on the code taking in consideration all the very useful  info you both shared with 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 :)

2018-08-17 05:37:13
valentina
2018-08-20 07:53:36 Kurt

An FYI for anyone who might have been following this.  The conversation has continued on the Github Issues page:

https://github.com/ROBOTIS-GIT/OpenCM9.04/issues/31#issuecomment-414140481


2018-08-20 07:53:36
Kurt
2018-08-21 06:27:09 Kurt

I think I am done playing with it.  I have a version that appears to be working.  I also updated it on the github link above.

My current test program code looks like:


#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:%dn", 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:%un", 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 CCWn", ax_moving_speed);
      else Serial.printf("New speed %d CWn", 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));
  }
}
 

The code tries to find transitions around the position of 512 as the transition from 1023 to 0 has issues with 60 degrees of no valid position information.  so this code tries to find values before 512 and values after 512 with sort of a no read area of 511-512, to signal that we had a revolution...  The code allows you to type in a new speed value, also allows you to type in P or p to toggle position value output on or off.


Sample 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
 

 Again I probably won't play much more with this, hopefully it works for you.


Kurt

2018-08-21 06:27:09
Kurt
Reply
웹에디터 시작 웹 에디터 끝