Results 1 to 2 of 2

Thread: Problem using AX-12A REG_WRITE & ACTION commands on ArbotiX--M board

  1. Problem using AX-12A REG_WRITE & ACTION commands on ArbotiX--M board

    I would like to use the REG-WRITE command first to send position commands to a group of Dynamixel AX-12A servos, and then to use the ACTION command to have them all respond simultaneously, as described in the AX-12A Manual in Section 4, Instruction Set and Examples. I have written a small program to test this. It inputs a servo ID via the Serial Monitor and then makes the servo move to a series of positions. This program runs OK when I use the WRITE-DATA command, but not when I use the REG_WRITE & ACTION commands. The program is shown below. I put the code for the necessary additional ax12 functions into my code; it is a modification of code in the ax12 library. There are lots of messages and delays so I can study the output on the Serial Monitor.

    Code:
    /***************************
     * Modification of AXSimpleTest by Ted Kurtz
     * This sketch sends positional commands to one AX servo
     * The sketch will send a value, i, to the servo.
     * 'For' loops are used to increment and decrement the value of 'i'
     ***************************/
    // This version requests the servo ID  14 Sept 2015
    // The arbotix-m board seems to require this to be COMPILED & uploaded each time it is run
    // The recvWithEndMarker() function was copied from the Arduino StringToInt example
    // Reference: http://forum.arduino.cc/index.php?topic=288234.0
    
    //Need the ax12 library to send DYNAMIXEL commands
    #include <ax12.h>  // NOTE!  arbotix-m board necessary for using this library
    
    //###############################################################
    // See code from ax12.h & ax12.cpp
    
    void ax12ActionSetRegister(int id, int regstart, int data);
    void ax12ActionSetRegister2(int id, int regstart, int data);
    
    // Some macros
    #define ActionSetPosition(id, pos) (ax12ActionSetRegister2(id, AX_GOAL_POSITION_L, pos))
    #define Action() (ax12SetRegister(0xfe, AX_ACTION, 0))
    // Set the value of a single-byte register in standby state.
    void ax12ActionSetRegister(int id, int regstart, int data){
        setTX(id);    
        int checksum = ~((id + 4 + AX_REG_WRITE + regstart + (data&0xff)) % 256);
        ax12writeB(0xFF);
        ax12writeB(0xFF);
        ax12writeB(id);    //                Byte 1
        ax12writeB(4);    // length = 4
        ax12writeB(AX_REG_WRITE);    // Byte 2
        ax12writeB(regstart);    //        Byte 3
        ax12writeB(data&0xff);    //     Byte 4
        // checksum = 
        ax12writeB(checksum);
        setRX(id);
        //ax12ReadPacket();
    }
    // Set the value of a double-byte register in standby state.
    void ax12ActionSetRegister2(int id, int regstart, int data){
        setTX(id);    
        int checksum = ~((id + 5 + AX_REG_WRITE + regstart + (data&0xFF) + ((data&0xFF00)>>8)) % 256);
        ax12writeB(0xFF);
        ax12writeB(0xFF);
        ax12writeB(id);    //                  Byte 1
        ax12writeB(5);    // length = 5
        ax12writeB(AX_REG_WRITE);    //   Byte 2
        ax12writeB(regstart);    //          Byte 3
        ax12writeB(data&0xff);    //          Byte 4
        ax12writeB((data&0xff00)>>8); // Byte 5
        // checksum = 
        ax12writeB(checksum);
    //    setRX(id);
        //ax12ReadPacket();
    }
    
    /* Give command to act on all pending AX_REG_WRITE commands. */
    void ax12Action(){
        int id = 0xfe;    // Broadcast ID
        setTX(id);    
        int checksum = ~((0xfe + 2 + AX_REG_WRITE) % 256);
        ax12writeB(0xFF);
        ax12writeB(0xFF);
        ax12writeB(id);    //                Byte 1
        ax12writeB(2);    // length = 4
        ax12writeB(AX_REG_WRITE);    // Byte 2
        // checksum = 
        ax12writeB(checksum);
        //setRX(id);    // No status pack is sent with this command
        //ax12ReadPacket();
    }
    //###############################################################
    
    
    int servoID;
    const byte numChars = 4;  // ID 4 chars max including end-of-string char
    char receivedChars[numChars];
    
    boolean newData = false;
    
    // The ArbotiX-M board does not seem to run setup() after a Ctrl-M command
    void setup(){
      Serial.begin(9600);
      delay(5000);  // Delay for activating serial monitor (Ctrl-Shift M) after uploading
      servoID = 0;  // Show not ready yet for testing servo
    }
    
    void loop(){
      Serial.println("Entering loop().");
      Serial.println("Input the servo ID:");
      delay(1000);
      if (Serial.available() > 0) {
        recvWithEndMarker();
        processData();
        delay(1000);
        if(servoID > 0){
          Serial.print("Testing: servoID = "); Serial.println(servoID);
          delay(100);
          //increment from 0 to 9
          for(int i=0; i<10; i++){
            Serial.println("Action-setting position."); delay(100);
            SetPosition(servoID,i*100); //set the position of servo to the current value of 'i'
    //        ActionSetPosition(servoID,i*100); //set the position of servo to the current value of 'i'
            Serial.println("Position-Set command given.");
            delay(3000);
            Serial.println("Now giving the action() command.");
    //        ax12Action();
            delay(1000);
            Serial.println("Action command completed"); Serial.println(); delay(1000);
          }
          SetPosition(servoID,512); //set the position of servo to '0'
          delay(10);//wait for servos to move
          delay(3000);  // Wait to give time to observe
        }
      }
    }
    
    void recvWithEndMarker() {
      static byte ndx = 0;
      char endMarker = '\n';  // new line char.  Set Serial Monitor to Newline2  
      char rc;
      Serial.println("In recvWithEndMarker()");
      if (Serial.available() > 0) {
        while (Serial.available() > 0 && newData == false) {
          rc = Serial.read();
          Serial.print("In Serial.read() rc = "); Serial.println(rc);  
          if (rc != endMarker) {
            receivedChars[ndx] = rc;
            ndx++;
            if (ndx >= numChars) ndx = numChars - 1;
          }
          else {
            receivedChars[ndx] = '\0'; // terminate the string
            ndx = 0;
            newData = true;
          }
        }
      }
    }
    
    void processData() {
      if (newData) {
        Serial.print("Received... ");
        Serial.println(receivedChars);
        servoID = atoi(receivedChars);
        newData = false;
      }
    }
    Has anyone been able to use the REG_WRITE & ACTION commands? Can anyone find why my code won't work with the REG_WRITE & ACTION commands enabled?

    Thank you.
    Ted

  2. Re: Problem using AX-12A REG_WRITE & ACTION commands on ArbotiX--M board Fixed!

    I found and fixed the problem I was having with the REG_WRITE & ACTION commands, a simple bug. Here is code that should work for any number of servos, limited only by the availability of sufficient power. I've tested it with nine unloaded servos using a three-cell LiPo 2600mAh battery such as used for RC helicopters. There are long delays so you can examine the servos as they move.

    Code:
    /***************************
     * Ted Kurtz  17 September 2015
     * Anyone welcome to copy or use this code freely
     ***************************/
    #include <ax12.h>  // NOTE!  arbotix-m board necessary for using this library
    
    //###############################################################
    // See code from ax12.h & ax12.cpp
    // Ad some code to it.
    
    //void ax12ActionSetRegister(int id, int regstart, int data);
    //void ax12ActionSetRegister2(int id, int regstart, int data);
    
    // Some macros
    #define ActionSetPosition(id, pos) (ax12ActionSetRegister2(id, AX_GOAL_POSITION_L, pos))
    #define Action() (ax12SetRegister(0xfe, AX_ACTION, 0))
    
    // Set the value of a single-byte register in standby state.
    void ax12ActionSetRegister(int id, int regstart, int data){
        setTX(id);    
        int checksum = ~((id + 4 + AX_REG_WRITE + regstart + (data&0xff)) % 256);
        ax12writeB(0xFF);
        ax12writeB(0xFF);
        ax12writeB(id);    //                Byte 1
        ax12writeB(4);    // length = 4
        ax12writeB(AX_REG_WRITE);    // Byte 2
        ax12writeB(regstart);    //        Byte 3
        ax12writeB(data&0xff);    //     Byte 4
        // checksum = 
        ax12writeB(checksum);
        setRX(id);
        //ax12ReadPacket();
    }
    // Set the value of a double-byte register in standby state.
    void ax12ActionSetRegister2(int id, int regstart, int data){
        setTX(id);    
        int checksum = ~((id + 5 + AX_REG_WRITE + regstart + (data&0xFF) + ((data&0xFF00)>>8)) % 256);
        ax12writeB(0xFF);
        ax12writeB(0xFF);
        ax12writeB(id);    //                  Byte 1
        ax12writeB(5);    // length = 5
        ax12writeB(AX_REG_WRITE);    //   Byte 2
        ax12writeB(regstart);    //          Byte 3
        ax12writeB(data&0xff);    //          Byte 4
        ax12writeB((data&0xff00)>>8); // Byte 5
        // checksum = 
        ax12writeB(checksum);
    //    setRX(id);
        //ax12ReadPacket();
    }
    
    // Give command to act on all pending AX_REG_WRITE commands.
    void ax12Action(){
        int id = 0xfe;    // Broadcast ID
        setTX(id);    
    //    int checksum = ~((0xfe + 2 + AX_REG_WRITE) % 256);
        int checksum = ~((0xfe + 2 + AX_ACTION) % 256);
        ax12writeB(0xFF);
        ax12writeB(0xFF);
        ax12writeB(id);    //                Byte 1
        ax12writeB(2);    // length = 4
        ax12writeB(AX_ACTION);    // Byte 2
        // checksum = 
        ax12writeB(checksum);
        //setRX(id);    // No status pack is sent with this command
        //ax12ReadPacket();
    }
    //###############################################################
    
    
    // The ArbotiX-M board does not seem to run setup() after a Ctrl-M command
    void setup(){
      Serial.begin(9600);
      delay(5000);  // Delay for activating serial monitor (Ctrl-Shift M) after uploading
    }
    
    #define NSERVOS 12  // Any value should be OK.  Missing servos can't respond!
    void loop(){
      for(int i=0; i<10; i++){  // Ten position commands
        Serial.println("Action-setting positions."); delay(100);
        for(int servoID = 1; servoID <= NSERVOS; servoID++){  // Send commands to each servo
          ActionSetPosition(servoID,i*100); //set the position of servo for the current value of 'i'
        }
        Serial.println("Position-Set commands given.");
        delay(3000);
        Serial.println("Now giving the action() command.");
        ax12Action();  // Make all servos simultaneously execute command
        delay(1000);
        Serial.println("Action command completed"); Serial.println(); delay(1000);
      }
      Serial.println("Center all servos.");
      delay(100);//wait for servos to move
      for(int servoID = 1; servoID <= NSERVOS; servoID++){  // Send commands to each servo
        ActionSetPosition(servoID,512); //set the position of the servo to center
      }
      ax12Action();  // Make all servos simultaneously execute command
      delay(3000);  // Wait to give time to observe
    }
    Ted

Thread Information

Users Browsing this Thread

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

Similar Threads

  1. Problem getting started with Arbotix-M
    By TedKurtz in forum Software and Programming
    Replies: 2
    Last Post: 08-20-2015, 01:41 PM
  2. Problem with Arbotix ADC
    By robot$ in forum Humanoids, Walkers & Crawlers
    Replies: 10
    Last Post: 05-29-2014, 02:46 PM
  3. Question(s) ArbotiX with LabVIEW: Low-level ArbotiX Commands?
    By vogonpoet42 in forum Software and Programming
    Replies: 2
    Last Post: 03-01-2013, 10:25 AM
  4. Question(s) Arbotix FTDI command problem
    By sarendt in forum Software and Programming
    Replies: 2
    Last Post: 03-14-2011, 01:19 PM
  5. Multiple Phidget 8/8/8 board problem
    By askinner in forum Arbotix, Microcontrollers, Arduino
    Replies: 10
    Last Post: 07-03-2007, 06:58 AM

Posting Permissions

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