Results 1 to 4 of 4

Thread: Problem to reed torque value from Dynamixel

  1. Problem to reed torque value from Dynamixel

    Hi again, everybody!
    As some of you already know I am improving a program to control a robotic arm with four dynamixel motors (2 MX64 and 2 mx106) in Arduino.


    My biggest problem is to control the torque, what I want is to make a collaborative robot, and for that, I need to know the torque that the robot will have everytime, so in the case that the torque is too high, to stop the robot.


    My program, today, is the following: (in this case I have set a torque limit of 500 and a serial.print of "Warning torque is exceed", as a test)
    Code:
    #include <ax12.h> //Include ArbotiX DYNAMIXEL library
    const int SERVO_ID[] = {1, 2, 3, 4}; 
    const int servo_count = sizeof(SERVO_ID) / sizeof(*SERVO_ID); //the compiler calculate the amount of servos
    int repetitions, interrupt_state;
    double Speed;
    int m = 400; //columns
    int pos2[] = {3820, 400, 230, 2395}; //rest position of the dynamixel motors
    void setup()
    {
      dxlInit(1000000); //start dynamixel library at 1mbps to communicate with the servos
      Serial.begin(9600); //start serial at 9600 for reporting data.
    
    
      for (int i = 0; i < servo_count; ++i)
      {
        Relax(SERVO_ID[i]);
        Serial.print(F("ID: "));
        Serial.println(SERVO_ID[i]);
      }
    
    
      delay(1000);
    
    
    }
    
    
    void loop()
    {
    
    
      Serial.println(F("How many times do you want to repeat the sequence?"));
    
    
      while (Serial.available() == 0) {}
      repetitions = Serial.parseInt();
      Serial.print(F("repetitions = "));
      Serial.println(repetitions);
      delay(3000);
    
    
      Serial.println(F("How fast do you want to repeat the sequence?"));
      Serial.println(F(" 1 = Slow     2  = Normal     3 = Fast"));
    
    
      while (Serial.available() == 0) {}
      Speed = Serial.parseInt();
    
    
      if (Speed <= 0 || Speed >= 4)
      {
        Serial.println(F("You have entered a wrong value"));
        return;
      }
      if (Speed == 1)
      {
        Speed = 1000;
      }
      else if (Speed == 2)
      {
        Speed = 700;
      }
      else if (Speed == 3)
      {
        Speed = 400;
      }
      Serial.print(F("Speed = "));
      Serial.print(Speed, 0);
      Serial.println(F(" microseconds"));
      delay(3000);
    
    
      Serial.print(F("Positions vector "));
      Serial.print(F(": ["));
    
    
      int positionn[servo_count][m]; //Matrix of movements
    
    
      for (int i = 0; i < m; i++) // structure to create columns
      {
        for (int j = 0; j < servo_count; j++) // structure to create columns
        {
          positionn[j][i] = dxlGetPosition(SERVO_ID[j]); //read and save the actual position
        }
        delay(10);
    
    
        for (int j = 0; j < servo_count; j++) // structure to create columns
        {
          Serial.print(positionn[j][i]); //Display the vector
          Serial.print(F(", "));
        }
      }
      Serial.print(F("]\n"));
      delay(5000);
    
    
      Serial.println(F("The servos will move to the initial position."));
      /***The servos will move according to registered movements***/
    
    
      for (int a = 0; a < repetitions; a++) //Repetition of the process (a = number of sequences)
      {
        Serial.print(F("SEQUENCE "));
        Serial.println(a + 1);
    
    
        int position[servo_count];
        int turns[servo_count];
        int pos1[servo_count];
        int pos2[servo_count];
        int current[servo_count];
        int torque_limit = 500;
        int torque[servo_count];
    
    
        for (int i = 0; i < servo_count; i++)
        {
          current[i] = positionn[i][0];
          position[i] = positionn[i][0];
          turns[i] = 0;
          pos1[i] = dxlGetPosition(SERVO_ID[i]); //Actual servo position
          pos2[i] = positionn[i][0]; //Initial position of the movement (objective)
        }
        for (int servo = 0; servo < servo_count; ++servo)
        {
          go_to_position(pos1, pos2, servo); //Function that moves the robot to the initial position
        }
    
    
        Serial.println(F("Now the servos will do the registered movements."));
        delay(2000);
    
    
        for (int movement = 0; movement < m; movement++)
        {
          for (int servo = 0; servo < servo_count; servo++)
          {
            if (positionn[servo][movement] != current[servo])
            {
              int next_pos = 1;
              if (positionn[servo][movement] < current[servo])
                next_pos = -1;
              while (positionn[servo][movement] != current[servo])
              {
                current[servo] += next_pos;
                delayMicroseconds(Speed);
                
                dxlSetGoalPosition(SERVO_ID[servo], current[servo]);
                torque[servo] = dxlGetTorque(SERVO_ID[servo]);
                Serial.println(torque[servo]);
                if (torque[servo] > torque_limit)
                {
                  Serial.println("WARNING torque limit exceed");
                }
                if (current[servo] == position[servo] + MX_MAX_POSITION_VALUE)
                {
                  position[servo] = current[servo];
                  turns[servo]++;
                }
                else if (current[servo] == position[servo] - MX_MAX_POSITION_VALUE)
                {
                  position[servo] = current[servo];
                  turns[servo]--;
                }
              }
            }
          }
        }
        for (int i = 0; i < servo_count; i++)
        {
          Serial.print(F("Turns engine "));
          Serial.print(i + 1);
          Serial.print(F(": "));
          Serial.println(turns[i]);
          Serial.println(" ");
        }
      }
      delay(3000);
    
    
      /****REST POSITION****/
      Serial.println(F("The robot will move to the resting position."));
      int pos1[servo_count];
      for (int i = 0; i < servo_count; i++)
      {
        pos1[i] = dxlGetPosition(SERVO_ID[i]); //Actual servo position
      }
      for (int servo = 0; servo < servo_count; ++servo)
      {
        go_to_position(pos1, pos2, servo);  //Function that moves the robot to the initial position
      }
      delay(1000);
      dxlTorqueOffAll();
      Serial.println(F("END!"));
    }
    void go_to_position(int pos1[], int pos2[], int servo)//function
    {
      while (pos1[servo] != pos2[servo])
      {
        if (pos2[servo] < pos1[servo])
        {
          dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
          pos1[servo]--;
          delayMicroseconds(800);
        }
        else if (pos2[servo] > pos1[servo])
        {
          dxlSetGoalPosition(SERVO_ID[servo], pos1[servo]);
          pos1[servo]++;
          delayMicroseconds(800);
        }
      }
    }

    The problem is that when the robot is moving, when the program prints on the serial monitor the torque that the motors support at all times:
    Code:
     torque[servo] = dxlGetTorque(SERVO_ID[servo]);
                Serial.println(torque[servo]);

    Almost always gives me a value of -1, and as much as I try to put some obstacle to the robot (with the intention of raising the torque and the message "Warning torque is exceed"), usually does not usually vary the value.


    So, I do not know if maybe using dxlGetTorque (ID) is not the correct option, or if what fails is that the ArbotiX-M that is not able to process as much data as position and torque at the same time or i don´t know ...


    I hope someone can give me their opinion !!
    Many thanks!!

  2. #2

    Re: Problem to reed torque value from Dynamixel

    The dxlGetTorque simply translatest to:
    #define dxlGetTorque(id) (dxlGetRegister(id, AX_PRESENT_LOAD_L, 2))

    Which when -1 implies that the dxlGetRegister failed. Why? That is always the hard thing to debug. Maybe packets being output faster than the units can respond? ...

    What do I do in cases like this? You can probably already guess from previous posts.... I hook up logic analyzer, and watch the packets to see if they are being sent properly, and if the servo is trying to respond? ... I might also instrument the code with digitalWrite calls around some of these, so we can see when the command is issued and exactly when it returned...

    I would also check things like, have all of the return delays of the servos been set to 0 (register 5). Might try checking if adding a small delay between calls helps...

    Would check to see if your servos are setup for Protocol 1 or protocol 2? I am assuming protocol 1, if they work at all, but if you are protocol 2, your servo register tables are different... But than again so are the packets...

    But if you go to Protocol 2, then you might be able to use things like SyncRead for reading the Present Current or the like, but everything is different... Also not sure if you have support for this with Arbotix board... Do with OpenCM...

  3. Re: Problem to reed torque value from Dynamixel

    Thank you for answering KurtEck,

    First of all, yes my servos are protocol 1, and yes I have an arobtix-m board to conect the servos with the computer, I´am working with arduino.

    I would also check things like, have all of the return delays of the servos been set to 0 (register 5). Might try checking if adding a small delay between calls helps...
    I tried to do that, but the program have done the same...

    I hook up logic analyzer, and watch the packets to see if they are being sent properly, and if the servo is trying to respond?
    I do not understand what you mean, sorry. At the moment I tried to do the following test:
    I have done this basic program:

    The program calculates the torque of the motors everytime, that is, when I move the robotic arm the different torques change. With which I deduce that the dxlGetTorque (id) block works correctly.

    I would say, seeing how the general program responds, that having to be calculating the position and also the torque of each motor, which is not able to process everything at once ... maybe I should put some delay after wanting to print the torque? maybe Is that part of the code too fast?

    I know that my SRAM memory of the microcontroller is almost full (as you know I´am triying to save the positions in a SD) when I say to the program to save 400 movements, but the thing is that the program also works baddly (also put -1 when is learning the torque) if I only say to save 10 movements. So, I didn´t think that it can be a memory problem no?

    Urko.

  4. Re: Problem to reed torque value from Dynamixel

    This is the basic program of the torque that I have mentiones in the previous post:

    Code:
    #include <ax12.h> 
    int SERVO_ID[] = {1, 2, 3, 4}; //números ID de los motores que se están utilizando
    int regData5, regData6, regData7, regData8;
    int u = 400; //columns
    int v = 1; //rows
    int w, z, j=0;
    
    
    
    
    
    
    void setup() {
    
    
      dxlInit(1000000); //start dynamixel library at 1mbps to communicate with the servos
      Serial.begin(9600); //start serial at 9600 for reporting data.
    
    
      Relax(SERVO_ID[0]);
      Relax(SERVO_ID[1]);
      Relax(SERVO_ID[2]);
      Relax(SERVO_ID[3]);
    
    
      Serial.print("ID: ");
      Serial.println(SERVO_ID[0]);
      Serial.print ("ID: ");
      Serial.println(SERVO_ID[1]);
      Serial.print("ID: ");
      Serial.println(SERVO_ID[2]);
      Serial.print("ID: ");
      Serial.println(SERVO_ID[3]);
      delay(1000);
    
    
    }
    
    
    void loop()  {
      
    int torque1[v][u];
    int torque2[v][u];
    int torque3[v][u];
    int torque4[v][u];
     
    while (j<1){
      
      for (w = 0; w < v; w++) 
      {
        Serial.print(" torque vector");
        Serial.print(w+1);
        Serial.print(": [");
    
    
        for (z = 0; z < u; z++) 
        {
    
    
            regData5 = dxlGetTorque(SERVO_ID[0]);
            regData6 = dxlGetTorque(SERVO_ID[1]);
            regData7 = dxlGetTorque(SERVO_ID[2]);
            regData8 = dxlGetTorque(SERVO_ID[3]);
    
    
            torque1[w][z] = regData5; 
            torque2[w][z] = regData6;
            torque3[w][z] = regData7;
            torque4[w][z] = regData8;
    
    
            delay(20);
    
    
            Serial.print(torque1[w][z]); //print the torque vector
            Serial.print(", ");
            Serial.print(torque2[w][z]);
            Serial.print(", ");
            Serial.print(torque3[w][z]);
            Serial.print(", ");
            Serial.print(torque4[w][z]);
            Serial.print("; ");
        }  
     }
             delay(5000);
             Serial.println("END");
             j++;
    }
      }

Thread Information

Users Browsing this Thread

There are currently 1 users browsing this thread. (0 members and 1 guests)

Similar Threads

  1. How it can be done a test of torque in Dynamixel
    By urko_18 in forum DYNAMIXEL & Robot Actuators
    Replies: 3
    Last Post: 4 Weeks Ago, 10:49 AM
  2. Dynamixel torque control using N.m
    By Roque in forum DYNAMIXEL & Robot Actuators
    Replies: 6
    Last Post: 09-11-2016, 11:51 AM
  3. Dynamixel error - no torque + red light blinking
    By panthallion in forum HR-OS1 Development and Discussion
    Replies: 16
    Last Post: 05-01-2015, 07:16 AM
  4. Question(s) How to Set the Torque limit for Dynamixel MX-28 using MATLAB?
    By ubs3 in forum Software and Programming
    Replies: 1
    Last Post: 03-06-2015, 08:41 PM
  5. Wireless Reed Switch?
    By SkipFire in forum Sensors
    Replies: 7
    Last Post: 05-10-2008, 06:53 PM

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •