Controlling AX or RX servos using arbotix

[tician]

Possibly an itty bitty typo (capitalization of bioloid in loop()):

//sets servo position using 2 numbers one for servo, other for position.
Bioloid.SetPosition(servoNum,servoPos);

Also, I haven't seen 'i' been declared anywhere before use:
either

char cmd[8];
int startByte = 0;
int i;

or (within both for loops)

for (int i=0; i<8; i++)...

On a side note to fergs, perhaps with your considerable programming experience you can enlighten me. Why do so many programmers use the

if (...){
...
...
}

instead of

if (...)
{
...
...
}

For some reason, it always seems to make me want to stab a sharp implement into my eyes whenever I encounter it as it seems less human readable to me.

[darkback2]

I'll try changing the capitalization error fix. The (


)

Vs
(

)

Is a funny thing, got help from my cousin in law. He learned it the first way, I learned it the second, and he said the first way was more readable to him. I think it may just come down to what you were raised with.

DB

[lnxfergy]

On a side note to fergs, perhaps with your considerable programming experience you can enlighten me. Why do so many programmers use the .... For some reason, it always seems to make me want to stab a sharp implement into my eyes whenever I encounter it as it seems less human readable to me.

Well, I suggest you never read real code then. Fact of the matter is that either will suffice for making code readable, and with limited screen space the first version can make more of your program visible at the same time.....

-Fergs

[Upgrayd]

Possibly an itty bitty typo (capitalization of bioloid in loop()):
For some reason, it always seems to make me want to stab a sharp implement into my eyes whenever I encounter it as it seems less human readable to me.

That seems like a reasonable reaction. It is just a personal preference thing but most prefer the compact version as it takes up less screen space.

[darkback2]

So given my limited understanding of any of this.

 while(Serial.available() < 8); // wait for data to arrive, do nothing      
      for(int i=0;i<8;i++){
        cmd[i] = Serial.read();
      }

So I added an int before the i stuff which solved that.

but I found a couple of problems that I probably should have noticed sooner. ax12.h is not orange like the bioloid stuff...I'll have to figure out where to put it to make that work.

DB

[lnxfergy]

ax12.h is not orange like the bioloid stuff...I'll have to figure out where to put it to make that work.

It won't be... it's not the name of the library, nor is defined as a keyword. Therefore, it'll be in plain black text.

-Fergs

[darkback2]

 //sets servo position using 2 numbers one for servo, other for position.
     bioloid.SetPosition(servoNum,servoPos);

results in this error

 In function 'void loop()':
error: expected unqualified-id before '(' token

I think I might have made bioloid.SetPosition up or something.

DB

[darkback2]

So I've now got the code compiling correctly, and SetPosition (id.pos) doesn't seam to be working properly. Here is the code.

#include <ax12.h>
#include <BioloidController.h>
#include "iPose01.h"  // exported from PyPose

BioloidController bioloid = BioloidController(1000000);

void setup()
{  
    // need to start the serial port
    Serial.begin(38400);

    // recommended pause
    delay(100);

    // stand up slowly    
    bioloid.loadPose(Up);
    bioloid.readPose();
    bioloid.interpolateSetup(500);
    
    while(bioloid.interpolating > 0)
    {
        bioloid.interpolateStep();
        delay(3);
    }
}

void loop() {
    char cmd[8];
    int startByte = 0;
    
    // if there are bytes waiting on the serial port
    if(Serial.available() > 0) {
      // read the incoming byte
      startByte = Serial.read();   
    } // end Serial.available()
    if(startByte == 'p') {
      // get the command from the serial port

// START FERGY EDITS HERE
      while(Serial.available() < 8); // wait for data to arrive, do nothing      
      for(int i=0;i<8;i++){
        cmd[i] = Serial.read();
      } //this creates an array that takes in the data from the serial port
// END THE FERGY EDITS
      
      // convert command to two numbers
      int servoNum = 0;
      int servoPos = 0;
      
      servoNum = cmd[0]*10 + cmd[1];
      servoPos = cmd[3]*1000 + cmd[4]*100 + cmd[5]*10 + cmd[6];
      
      //sets servo position using 2 numbers one for servo, other for position.  currently not working.
     SetPosition(servoNum,servoPos);
      
      // debug
      Serial.print("cmd array:");
      for(int i=0;i<8;i++){
        Serial.print(cmd[i]);
      }
      Serial.println("----");
      Serial.println("servoNum:"+servoNum);
      Serial.println("servoPos:"+servoPos);
      
    }
      
} // end loop()

When I uploaded it to the arbotix, if I send
"p19,0412" I get back
"cmd array:19,0512
----
" followed by a lot of garbage.

The robot doesn't do anything though, so I think I'm missing something crucial.

DB

[lnxfergy]

I'm honestly not sure how it compiles -- I don't think println can work like that (concatenating a string and an int, it's likely using the address and making a real memory mess instead). Try updating these two lines:

      Serial.println("servoNum:"+servoNum);
      Serial.println("servoPos:"+servoPos);

To this:

      Serial.print("servoNum:");
      Serial.println(servoNum);
       Serial.print("servoPos:");
      Serial.println(servoPos);

Also, you're sending ascii -- which is not the same as a binary number, so you have to change these lines:

      servoNum = cmd[0]*10 + cmd[1];
      servoPos = cmd[3]*1000 + cmd[4]*100 + cmd[5]*10 + cmd[6];

To something like this:

      servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
      servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);

Because the ASCII representation of 0 is 48, and 1 is 49, etc..

-Fergs

[darkback2]

At long last, Here it is. Thanks to everyone who contributed to this project. I'm also going to create a MAX/MSP patch that controls this code. Anyone should feel free to use it.

DB

/* ========================================================================== */
/*                          HikariKey/ServoControl
/*  This code is a good starting place for using the 
/*  keyboard of a computer to control servos using the arbotix.
/*  Send data in the form of p (servo ID 2 digets) , (goal position 4 digets) CR
/*  You can also send it letters from the keyboard and have it play                                                                          */
/*  sequences based on which letters you send it.                                                             */
/*  If you use this you want to change the "iPoses01.h to whatever 
/*  your poses are called.  You also want to change the if thens to 
/*  things that make sense to you.
/*  
/*  This code was written by Darkback2 with more help than not by
/*  Chris Novak
/*  lnxfergy (Michael Ferguson, inventor of the arbotix)
/*  Upgrayd
/*  and the rest of the Trossen Robotics Forum.
/* ========================================================================== */

#include <ax12.h>
#include <BioloidController.h>
#include "iPose01.h"  // exported from PyPose

BioloidController bioloid = BioloidController(1000000);

void setup()
{  
    // need to start the serial port
    Serial.begin(38400);

    // recommended pause
    delay(100);

    // stand up slowly    
    bioloid.loadPose(Up);
    bioloid.readPose();
    bioloid.interpolateSetup(500);
    
    while(bioloid.interpolating > 0)
    {
        bioloid.interpolateStep();
        delay(3);
    }
}

void loop() {
    char cmd[8];
    int startByte = 0;
    
    // if there are bytes waiting on the serial port
    if(Serial.available() > 0) {
      // read the incoming byte
      startByte = Serial.read(); 
      // toggle LED so we know johnny5 is alive
      digitalWrite(0,HIGH-digitalRead(0));  
    } // end Serial.available()
    if(startByte == 'p') {
      // get the command from the serial port
      while(Serial.available() < 8); // wait for data to arrive, do nothing      
      for(int i=0;i<8;i++){
        cmd[i] = Serial.read();
      } //this creates an array that takes in the data from the serial port      
      // convert command to two numbers
      int servoNum = 0;
      int servoPos = 0;
      
      servoNum = (cmd[0]-48)*10 + (cmd[1]-48);
      //builds the 2 diget servo Id number
      servoPos = (cmd[3]-48)*1000 + (cmd[4]-48)*100 + (cmd[5]-48)*10 + (cmd[6]-48);
      //builds the 4 diget goal position
     SetPosition(servoNum,servoPos);
     //sets servo position using 2 numbers one for servo, other for position.
      // debug
      Serial.print("cmd array:");
      for(int i=0;i<8;i++){
        Serial.print(cmd[i]);
      }
      Serial.println("----");
     Serial.print("servoNum:");
      Serial.println(servoNum);
       Serial.print("servoPos:");
      Serial.println(servoPos);
      
    }// end if(startByte == 'p') {
    else {
      //process commands other than servo positions
      // only process if we aren't currently doing a sequence
        if(bioloid.playing == 0)
        {               
            if(startByte == 'w') //if I press w walk forward
            {
                bioloid.playSeq(LRLean);
            }
            if(startByte == 'W') //if I press W walk forward slow
            {
                bioloid.playSeq(WalkForward03);
            }
            else if(startByte == 's') //if I press s stop moving
            {
                bioloid.playSeq(UP);    
            }
            else if(startByte == 'x') //if I press x defensive pose
            {
                bioloid.playSeq(rest01);    
            }
            else if(startByte == 'g') //if I press s get up facedown
            {
                bioloid.playSeq(facedown);    
            }
            else if(startByte == 'b') //if I press b roll over
            {
                bioloid.playSeq(backtofront);    
            }
            else if(startByte == 'q') //if I press q turn left
            {
                bioloid.playSeq(LeftTurn);    
            }
            else if(startByte == 'a') //if I press a sidestep left
            {
                bioloid.playSeq(SideLT);    
            }
            else if(startByte == 'e') //if I press e turn right
            {
                bioloid.playSeq(RightTurn);    
            }
            else if(startByte == 'd')//if I press d sidestep right
            {
                bioloid.playSeq(SideRT);
            }
      
            else if(startByte == 'j') //if I press j attack left
            {
                bioloid.playSeq(LA); 
            }
      
            else if(startByte == 'l') //if I press l attack right
            {
                bioloid.playSeq(RA);
            }
            else if(startByte == 'k') //if I press k right punch attack front
            {
                bioloid.playSeq(RightPunch);
            }
             else if(startByte == 'i') //if I press i front push
            {
                bioloid.playSeq(FFrontAttack);
            }
            else if(startByte == 'm') //if I press m restPunch
            {
                bioloid.playSeq(RestPunchCombo);
            }
            else if(startByte == 't') //if I press t restPunch
            {
                bioloid.playSeq(FDrop);
            }
            else if(startByte == '1') //if I press 1 dance 01
            {
                bioloid.playSeq(DanceS01);
            }
            else if(startByte == '2') //if I press 1 dance 02
            {
                bioloid.playSeq(DanceS02);
            }
            else if(startByte == '3') //if I press 3 dance 03
            {
                bioloid.playSeq(DanceS03);
            }
            else if(startByte == '4') //if I press 4 dance 04
            {
                bioloid.playSeq(DanceS04);
            }
            else if(startByte == '5') //if I press 5 dance 05
            {
                bioloid.playSeq(DanceS05);
            }
        } 
    
    
    if(bioloid.playing > 0)
    {
        // continue sequence
        bioloid.play();  
    }
      
    }// end else
} // end loop()