Control a dynamixel starting from accelerations value

Hi all, I am a very rookie in the Arduino environment.
I am planning to detect the accelerations of an OpenCR 1.0 thanks to its incorporated IMU and to move a dynamixel consequentially.
Thus, what I want is to mix the sketch of IMU_Read_AccRAW and that one of Dynamixel Workbench library, called j_Current_Based_Position.
When for example the x_acceleration overpasses a value then the dynamixel has to reach a specific angle.
I am sorry for my not complete knowledge about it, indeed some tip could be nice for sure.
By the way, the first sketch is built completely on the loop. Instead, the second sketch in the setup. I tried more ways to unify them but I never figured it out.
(I am able to turn on more less if the acceleration has some specific values but not to move a dynamixel)
Furthermore, I don’t understand the reason why I can’t put only the command " dxl_wb.goalPosition(dxl_id, (int32_t)0);" or " Serial.print(IMU.accRaw[0]);" without the init and the ping for the first and without the clock for the second.

#include <DynamixelWorkbench.h>

#if defined(__OPENCM904__)
  #define DEVICE_NAME "3" //Dynamixel on Serial3(USART3)  <-OpenCM 485EXP
#elif defined(__OPENCR__)
  #define DEVICE_NAME ""
#endif   

#define BAUDRATE  57600
#define DXL_ID    1

DynamixelWorkbench dxl_wb;

void setup() 
{
  Serial.begin(57600);
  while(!Serial); // Wait for Opening Serial Monitor

  const char *log;
  bool result = false;

  uint8_t dxl_id = DXL_ID;
  uint16_t model_number = 0;

  result = dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to init");
  }
  else
  {
    Serial.print("Succeeded to init : ");
    Serial.println(BAUDRATE);  
  }

  result = dxl_wb.ping(dxl_id, &model_number, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to ping");
  }
  else
  {
    Serial.println("Succeeded to ping");
    Serial.print("id : ");
    Serial.print(dxl_id);
    Serial.print(" model_number : ");
    Serial.println(model_number);
  }

  result = dxl_wb.currentBasedPositionMode(dxl_id, 30, &log);
  if (result == false)
  {
    Serial.println(log);
    Serial.println("Failed to change current based position mode");
  }
  else
  {
    Serial.println("Succeed to change current based position mode");
    Serial.println("Dynamixel is moving...");

    for (int count = 0; count < 3; count++)
    {
      dxl_wb.goalPosition(dxl_id, (int32_t)0);
      delay(3000);

      dxl_wb.goalPosition(dxl_id, (int32_t)2048);
      delay(3000);
    }
  }
}

void loop() 
{

}
/*
  Range   : +/- 2 g
  Scale   : 16384 = 1 g
 */

#include <IMU.h>


cIMU    IMU;



uint8_t   led_tog = 0;
uint8_t   led_pin = 13;

void setup()
{
  Serial.begin(115200);

  IMU.begin();

  pinMode( led_pin, OUTPUT );
}





void loop()
{
  static uint32_t tTime[3];
  static uint32_t imu_time = 0;


  if( (millis()-tTime[0]) >= 500 )
  {
    tTime[0] = millis();

    digitalWrite( led_pin, led_tog );
    led_tog ^= 1;
  }

  tTime[2] = micros();
  if( IMU.update() > 0 ) imu_time = micros()-tTime[2];



  if( (millis()-tTime[1]) >= 50 )
  {
    tTime[1] = millis();

    Serial.print(imu_time);
    Serial.print(" \t");
    Serial.print(IMU.accRaw[0]);    // ACC X
    Serial.print(" \t");
    Serial.print(IMU.accRaw[1]);    // ACC Y
    Serial.print(" \t");
    Serial.print(IMU.accRaw[2]);    // ACC Z
    Serial.println(" ");
  }
}

Welcome to the forum

Please follow the advice on posting code given in Read this before posting a programming question in order to make your sketch easy to read, copy and test

In particular note the advice to Auto format code in the IDE and to use code tags when posting code here as it prevents some combinations of characters in code being interpreted as HTML commands such as italics, bold or a smiley character, all of which render the code useless

If the code exceeds the 9000 character inline limit then attach it to a post

#include <DynamixelWorkbench.h>
#if defined(__OPENCM904__)
  #define DEVICE_NAME "3" //Dynamixel on Serial3(USART3)  <-OpenCM 485EXP
#elif defined(__OPENCR__)
  #define DEVICE_NAME ""
#endif   

#define BAUDRATE  57600
#define DXL_ID    1

DynamixelWorkbench dxl_wb;

#include <IMU.h>
cIMU    IMU;
uint8_t   led_tog = 0;
uint8_t   led_pin = 13;

void setup()
{
  Serial.begin(115200);
  IMU.begin();
  pinMode( led_pin, OUTPUT );
}

void loop() 
{
const char *log;
  bool result = false;
  uint8_t dxl_id = DXL_ID;
  uint16_t model_number = 0;
  result = dxl_wb.init(DEVICE_NAME, BAUDRATE, &log);
    result = dxl_wb.ping(dxl_id, &model_number, &log);
    result = dxl_wb.currentBasedPositionMode(dxl_id, 30, &log);
    static uint32_t tTime[3];
  static uint32_t imu_time = 0;
  if( (millis()-tTime[0]) >= 500 )
  {
    tTime[0] = millis();
    digitalWrite( led_pin, led_tog );
    led_tog ^= 1;
  }
  tTime[2] = micros();
  if( IMU.update() > 0 ) imu_time = micros()-tTime[2];
  if( (millis()-tTime[1]) >= 50 )
  {
    tTime[1] = millis();

    Serial.print(imu_time);
    Serial.print(" \t");
    Serial.print(IMU.accRaw[0]);    // ACC X
    Serial.print(" \t");
    Serial.print(IMU.accRaw[1]);    // ACC Y
    Serial.print(" \t");
    Serial.print(IMU.accRaw[2]);    // ACC Z
    Serial.println(" ");
  
  if (result != false  && (IMU.accRaw[0]>1000)
 
  {
    Serial.println("Succeed to change current based position mode");
    Serial.println("Dynamixel is moving...");

      dxl_wb.goalPosition(dxl_id, (int32_t)0);
     
      
    }
  }
}

I tried to put what my mind would want inside the code. Something like that.

Hi @zewrx, I've been checking in on related posts like this and wanted to let you know there is a new dynamixel-specific forum on community.robotis.us . If you have any other questions about your servos it could be a good resource to check out.

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.