PDA

View Full Version : [Question(s)] Controlling AX or RX servos using arbotix



darkback2
10-27-2010, 10:46 PM
Ok, so I tried to come up with a program that would take data from the serial port and use it to control AX-12 servos. The idea is that you send two numbers out the serial port, one being the servo the other being the position and the arbotix would move the servo that the desired position. Figured this would be useful to more than just me.


#include <ax12.h>
#include <BioloidController.h>
#include "poses.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(Start);
bioloid.readPose();
bioloid.interpolateSetup(500);

while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}

void loop()
{
// check for new messages over the serial port
//looking for 2 numbers set with a comma..."1,512"
//not sure int or x,y is even remotely close to workable.
//also not sure if I can input numbers greater than 9 this way.
int x,y = Serial.read()
//possibly better as int x = serial.read()
// int y = serial.read()
;
if( x,y >= 0 )
//if (x >= 0 ) and (y >=0 )
{
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));

// only process if we aren't currently doing a sequence
if(bioloid.playing == 0)
{
//sets servo position using 2 numbers one for servo, other for position.
Bioloid.SetPosition(x,y)

if(bioloid.playing > 0)
{
// continue sequence
bioloid.play();
}

}
}

DB

jes1510
10-28-2010, 11:30 AM
Nice work!

darkback2
10-28-2010, 11:58 AM
Nice work!

would be if it worked. :(

tician
10-29-2010, 01:43 AM
I hope I am not being an ass or overwhelming and/or confusing anyone, as I really am trying to be helpful. I just have never used an arbotix and am not so good at the whole "helping people to figure things out for themselves" thing.

A few things to remember about C/C++ in general: 1) all names are case-sensitive, 2) a semicolon is required at the end of each line of code, 3) braces make all the difference in execution order/occurrence.

About the code specifically, does 'Serial.read()' return a single byte (8-bit char), 2 bytes (16-bit short), or 4 bytes (32-bit integer)? Also, in what format will you be sending data over the serial connection (Binary, Binary Coded Decimal, or ASCII)?

For instance, if the 'Serial.read()' returns only a single byte and you want to use plain 8-bit ASCII characters as if you were typing on the keyboard, then you would use three bytes for the servo ID (up to 255 IDs allowed by dynamixels) and four bytes for the servo position (AX-12+ position values range from 0 to 1023).
This is certainly not the most efficient code and is ignoring the use of a ',' as delimiting the two values, but I was going mostly for human readable:


int x, x0, x1, x2;
int y, y0, y1, y2, y3;

x2 = (Serial.read()&0x000F)*100; // ignore the top nibble to convert to BCD and then put in hundreds
x1 = (Serial.read()&0x000F)*10; // ignore the top nibble to convert to BCD and then put in tens
x0 = (Serial.read()&0x000F); // ignore the top nibble to convert to BCD and then put in ones
x = x2 + x1 + x0; // sum the three decimal digits

y3 = (Serial.read()&0x000F)*1000; // ignore the top nibble to convert to BCD and then put in thousands
y2 = (Serial.read()&0x000F)*100; // ignore the top nibble to convert to BCD and then put in hundreds
y1 = (Serial.read()&0x000F)*10; // ignore the top nibble to convert to BCD and then put in tens
y0 = (Serial.read()&0x000F); // ignore the top nibble to convert to BCD and then put in ones
y = y3 + y2 + y1 + y0; // sum the four decimal digits
This code is assuming we are sending data with the Most Significant digit first, so a servo ID of "213" would be sent as three ASCII characters: '2', then '1', then '3'. If we are assuming that there will never be anything other than a single ASCII coded decimal digit sent at one time, then we can just ignore the top nibble (most significant 4-bits) as the decimal number '2' is represented in ASCII coding by 0x32 (0011 0010 in binary) (50 in decimal). A nice thing about ASCII is that the representations of the decimal digits '0' through '9' are simply the Binary Coded Decimal value in the lower nibble with 3 in the upper nibble. '2' is 0x32, '5' is 0x35, etc.
Confused yet?

Added a few basic fixes to your code, but it will still need work depending on what you want it to do.


#include <ax12.h>
#include <BioloidController.h>
#include "poses.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(Start);
bioloid.readPose();
bioloid.interpolateSetup(500);

while(bioloid.interpolating > 0)
{
bioloid.interpolateStep();
delay(3);
}
}

void loop()
{
// check for new messages over the serial port
//looking for 2 numbers set with a comma..."1,512"
//not sure int or x,y is even remotely close to workable.
//also not sure if I can input numbers greater than 9 this way.

// int x,y = Serial.read()
// If it is C/C++, then the line above declares two integer variables named 'x' and 'y', but calls the
// function Serial.read() only once and assigns the resulting value to the variable 'y'

// For whatever format you decide to send the Serial data stream in, the code would go here:
int x = Serial.read();
int y = Serial.read();

// C/C++? Could add parenthesis around each condition for readability
if ( x >= 0 && y >=0 )
{
// toggle LED so we know johnny5 is alive
digitalWrite(0,HIGH-digitalRead(0));

// only process if we aren't currently doing a sequence
if(bioloid.playing == 0)
{
//sets servo position using 2 numbers one for servo, other for position.
bioloid.SetPosition(x,y);
}

// Might want to change this to 'else' or 'else if' (bioloid.playing should have only two
// states/values '1' or '0'), but not sure it is really necessary to change it.
if(bioloid.playing > 0)
{
// continue sequence
bioloid.play();
}
}
}

darkback2
10-29-2010, 11:35 AM
Hey Tician,

First off thanks for your help with this.

I am trying to write a program that would allow a user to pass the arbotix two numbers through the serial port. For example 19,512 The arbotix would respond by moving servo 19 to position 512. We could later change it so that the arbotix would respond to different inputs for example 19,D300 would move servo 19 to 300 degrees...We could also use 19,R1.57 so that the arbotix would move servo 19 to 1.57 radians. That way anyone could write a program that could control servos on an arbotix relatively easily.

Second to the larger trossen community.

The idea here isn't to help someone help themself. The idea here is to get working code that any of us can use. I am trying to make the arbotix which is already dead simple to use more simple to use with other software. So...if you have a solution that would work in this situation, then please post it. I'm sorry if I sound snippy, I don't mean to and am greatful for any help that anyone can bring to the table.

DB

lnxfergy
10-29-2010, 11:47 AM
DB -- I have some older code that does a similar structure (except it used commands like "GET IR 1") -- same idea though, it'll probably only take me about ~15 minutes to fix it up for this task -- but I just haven't had time yet....

(Hopefully tonight I'll have a bit of time to do this, and I also wanted to update PyMech to use the new commander protocol, so that people with ArbotiX/NUKE based mechs have an out-of-the box control method, since I know at least Cire would be happy to not have to dig in and update the code...)

-Fergs

darkback2
10-29-2010, 11:58 AM
Thanks Fergs, Seams like we could all use it.

darkback2
11-27-2010, 11:14 PM
Hey Guys,

So...I had the fortune of having a really decent programmer (my cousin in law) come to my house for Thanks Giving. He helped me write the following for controlling servos using the arbotix. The structure is "p" servo number ","position "cr" and then it sets the servo position using that data. Tests resulted in nothing working, but I'm not sure I sent the data right. Here is our code. Any help would be appreciated.

DB


#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
if(Serial.available() >= 8) {
for(i=0;i<8;i++){
cmd[i] = Serial.read();
}
}

// 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.
Bioloid.SetPosition(servoNum,servoPos);

// debug
Serial.print("cmd array:");
for(i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.println("servoNum:"+servoNum);
Serial.println("servoPos:"+servoPos);

}

} // end loop()

lnxfergy
11-27-2010, 11:25 PM
From what I can see right off the bat DB, you probably want to change how you're looping. Currently, you are reading the first byte (start byte) but then checking to see if there are 8 more bytes available -- if not, you lose that start byte, rather than waiting for more bytes to arrive, I would do the following:


#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
// get rid of this... if(Serial.available() >= 8) {
for(i=0;i<8;i++){
cmd[i] = Serial.read();
}
//} also get rid of this....
// 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.
Bioloid.SetPosition(servoNum,servoPos);

// debug
Serial.print("cmd array:");
for(i=0;i<8;i++){
Serial.print(cmd[i]);
}
Serial.println("----");
Serial.println("servoNum:"+servoNum);
Serial.println("servoPos:"+servoPos);

}

} // end loop()There may be other issues -- but that's the one that jumps out to me real fast. See if that at least gets it doing something, then we could look further.

-Fergs

darkback2
11-28-2010, 02:49 AM
So now I'm getting this

error: redefinition of 'BioloidController bioloid' In function 'void setup()':
In function 'void loop()':

error. Just got back from a party. Unfortunately I won't be able to test this out until I get a new xbee...(just had a minor xbee mishap. Chris wants to deny helping me with my code because of what your edit...so it must be really good.

DB

tician
11-28-2010, 07:59 AM
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
11-28-2010, 09:41 AM
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
11-28-2010, 11:03 AM
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
11-28-2010, 11:19 AM
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
11-28-2010, 12:51 PM
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
11-28-2010, 12:57 PM
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
11-28-2010, 01:02 PM
//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
11-28-2010, 02:13 PM
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
11-28-2010, 05:43 PM
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
11-28-2010, 11:15 PM
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()

Slugman
12-01-2010, 06:02 PM
DB - Thanks so very much for sharing that code - Have already modified it to fit my bot, & it compiled with only a few de-bugs (It didn't like me using numbers to name my poses). I already gave you a +Rep when you shared part of the original code in a reply to one of my posts, but the only reason I'm not doing it again is because I am sure that's not in the spirit of the Reps, but REALLY good work, & doubly so for sharing! :happy:

I know I only seem to post to ask something, *cringe*......but is there any other Windows users out there who can point me towards a python program for sending the packets to the com port? Thanks to DB (& the others who contributed) I have the program at the receiving end, so now I just need to make/get the program at the PC end. I am reasearching Sockets in Python at the moment, so can anyone tell me if I am on the right track or not? :confused:
If the examples at the bottom of the page at http://docs.python.org/library/socket.html are what I am looking for, then a simple reply of "Yes you idiot, use them with something that grabs keystrokes from the keyboard." will suffice..... :wink:

lnxfergy
12-01-2010, 08:44 PM
I know I only seem to post to ask something, *cringe*......but is there any other Windows users out there who can point me towards a python program for sending the packets to the com port? Thanks to DB (& the others who contributed) I have the program at the receiving end, so now I just need to make/get the program at the PC end. I am reasearching Sockets in Python at the moment, so can anyone tell me if I am on the right track or not? :confused:
If the examples at the bottom of the page at http://docs.python.org/library/socket.html are what I am looking for, then a simple reply of "Yes you idiot, use them with something that grabs keystrokes from the keyboard." will suffice..... :wink:

You want to use PySerial to open a com port - not sockets. As for examples, there are quite a few for PySerial, and PyPose also uses it.

-Fergs

Slugman
12-01-2010, 09:20 PM
OK, thanks. :happy: I was going through the various tutes that exist for PyGame, which seem to have the info of what I need to learn.
I just need to put it all together now. It could take a while.

darkback2
12-02-2010, 12:59 AM
Hey Slugman,

seriously the real doing here was done by other people. I was really lucky to get all of the help I got from Chris Novak and Fergy. I honestly only typed a few words in putting this together. the cool part is I think its commented well enough that I actually understand what I did.

s for opening a serial port, you can start by using a the software you use to set up your xbees. I'll be putting together a visual program where you can trigger poses or individually move servos in the next few weeks...and more likely than not this weekend.

DB

darkback2
05-16-2011, 09:41 PM
Here is the latest bit I am working with. It can directly control servos and do poses.

/* ================================================== ======================== */
/* 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 "Hikarikey04.h" // exported from PyPose
BioloidController bioloid = BioloidController(1000000);

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

// recommended pause
delay(100);
delay(1000);
// do a search for devices on the RX bus, default to AX if not found
int i;
for(i=0;i<AX12_MAX_SERVOS;i++){
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}

for (int i = 1; i < 21; i++) { // det the compliance of all servos
ax12SetRegister(i, 0x1A, 6);
delay(3);
ax12SetRegister(i, 0x1B, 6);
delay(3);
}
//end new stuff from jess
}
// stand up slowly
bioloid.loadPose(up01);
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 == 'r') //if I press r relax the legs
{
Relax(5);
Relax(7);
Relax(9);
Relax(11);
Relax(6);
Relax(8);
Relax(10);
Relax(12);
}
else if(startByte == 'w') //if I press w walk forward
{
bioloid.playSeq(simplewalk01);
}
else if(startByte == 'W') //if I press W walk forward slowly
{
bioloid.playSeq(slowwalk01);
}
else if(startByte == 'X') //if I press W walk forward slowly
{
bioloid.playSeq(Fwalkprep01);
}
else if(startByte == 's') //if I press s stop moving
{
bioloid.playSeq(Jump01);
}
else if(startByte == 'S') //if I press s stop moving
{
bioloid.playSeq(Jump02);
}
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(Facedown01);
}
else if(startByte == 'G') //if I press s get up facedown
{
bioloid.playSeq(Facedown02);
}
else if(startByte == 'b') //if I press b roll over
{
bioloid.playSeq(Backup01);
}
else if(startByte == 'B') //if I press b roll over
{
bioloid.playSeq(Backup02);
}
else if(startByte == 'q') //if I press q turn left
{
bioloid.playSeq(JturnR01);
}
else if(startByte == 'a') //if I press a sidestep left
{
bioloid.playSeq(Sl01);
}
else if(startByte == 'e') //if I press e turn left
{
bioloid.playSeq(JturnL01);
}
else if(startByte == 'd')//if I press d sidestep right
{
bioloid.playSeq(Sr01);
}
else if(startByte == 'j') //if I press j attack left
{
bioloid.playSeq(LeftAttack01);
}
else if(startByte == 'l') //if I press l attack right
{
bioloid.playSeq(RightAttack01);
}
else if(startByte == 'k') //if I press k right punch attack front
{
bioloid.playSeq(Push01);
}
else if(startByte == 'K') //if I press k right punch attack front
{
bioloid.playSeq(Push02);
}
else if(startByte == 'i') //if I press i front push
{
bioloid.playSeq(AttackFront01);
}
else if(startByte == 'z') //if I press z rest
{
bioloid.playSeq(Defence01);
}
// I ran into some trouble here. The numbers were getting
// triggered when I was sending servo positions.


else if(startByte == 'n') //if I press n backfall
{
bioloid.playSeq(DownPush01);
}
else if(startByte == 'N') //if I press n Downpush 02
{
bioloid.playSeq(DownPush02);
}
else if(startByte == 'm') //if I press n backfall
{
bioloid.playSeq(RestAttack01);
}
else if(startByte == '.') //if I press "." fire the right gun
{
ax12SetRegister2(25, AX_GOAL_SPEED_L,512);
ax12SetRegister2(26, AX_GOAL_SPEED_L,512);
}
else if(startByte == ',') //if I press "," stop shooting
{
ax12SetRegister2(25,AX_GOAL_SPEED_L,0);
ax12SetRegister2(26,AX_GOAL_SPEED_L,0);
}
else if(startByte == '"') //if I press """ fire the right gun
{
ax12SetRegister2(23, AX_GOAL_SPEED_L,512);
}
else if(startByte == ':') //if I press ":" stop shooting
{
ax12SetRegister2(23,AX_GOAL_SPEED_L,0);
}
else if(startByte == '}') //if I press "" fire the right gun
{
ax12SetRegister2(24, AX_GOAL_SPEED_L,512);
}
else if(startByte == '{') //if I press "{" stop shooting
{
ax12SetRegister2(24,AX_GOAL_SPEED_L,0);
}
}


if(bioloid.playing > 0)
{
// continue sequence
bioloid.play();
}

}// end else
} // end loop()

Slugman
05-16-2011, 10:18 PM
FYI - Still using your original program for my 22 DoF Hex - It works really well :happy: Just been stuffing about with the Computer side of things - 2 days ago I finally got to move the individual servos of Crabbys front legs with sliders on the screen so I can grab things with the claws - A bit twitchy, but I should be able to work that out. I just have to change everything into two loops - One for walking, & then one for claw manipulation. :veryhappy:

So just clarifying (& finally getting to my question), the command;

ax12SetRegister2(24, AX_GOAL_SPEED_L,512);

sets the gun motor running, & the other one stops it, so does that mean that I could use that as a signal to a speed controller for running a motor with a higher current drain than 1Amp? or can that signal only be used for Robotis servos.
I.E. Are your guns using Robotis servos, or does the Arbotix directly drive the Tank/Airsoft guns using the motor drivers? I'm just a little confused since the numbers in your program range from 23 to 26, which don't seem to match up to the output pins on the Arbotix, unless I've got it wrong. I read the above example to mean that Servo No.24 will have a goal speed set to 512. Is that right?

darkback2
05-17-2011, 03:05 PM
Hey Slugman,

That is precisely what I am doing. I am running two hopped up tank guns, using the guts from AX-12 servos. There is talk about making a cheaper board in order to accomplish this down the line, but I had more AX-12s than I did time.

I'll post pics of what I did this evening.

DB

horma
06-27-2011, 05:37 AM
Hi darkback2,

First congratulations for your work.

When compiling the code gives me an error:


In function 'void setup()':
error: 'dynamixel_bus_config' was not declared in this scope In function 'void loop()':

which version to use the library BioloidController?

You know it could be happening?

Thanks for your help

lnxfergy
06-27-2011, 12:27 PM
Horma,

DB is using the ArbotiX2 beta version of the BioloidController library. If you remove the following code:



for(i=0;i<AX12_MAX_SERVOS;i++){
dynamixel_bus_config[i] = 1;
if(ax12GetRegister(i+1, AX_ID, 1) != (i+1)){
dynamixel_bus_config[i] = 0;
}


It should work with previous versions.

-Fergs

horma
06-28-2011, 03:02 AM
Hi lnxfergy,

Thank you very much.

works fine.

darkback2
07-12-2011, 04:44 PM
Ok...so I'm trying to use the commander to do several different things, I'm trying to make it so that buttons have multiple functions that are conditional on other buttons.

Here is what I did, but for some reason nothing happens at all now.




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

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

void setup(){

//open the serial port
command.begin(38400);
// stand up slowly
delay(1000); // recommended pause
bioloid.loadPose(up01);
bioloid.readPose();
bioloid.interpolateSetup(500);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
//variables for looking.

}
}

void loop(){
int koolx;
koolx = 512;
int kooly;
kooly = 512;
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence
// if no buttons are pressed then the walk and look joysticks
//move the robot around.
if(command.buttons&BUT_R1 == 0) {
if(command.buttons&BUT_L6 == 0) {
if(command.walkV > 50){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk Crouch Down
bioloid.playSeq(Defence01);
}else if(command.walkH > 50){
// turn left
bioloid.playSeq(JturnL01);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(JturnR01);
}else if(command.lookH > 50){
// turn right
bioloid.playSeq(JturnL01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(JturnR01);
}
}
}
// press the first button on the right side
// This turns the joystick into an attack commander.
if(command.buttons&BUT_R1 > 0) {
if(command.buttons&BUT_L6 == 0) {
if(command.walkV > 50){
// Front attack
bioloid.playSeq(AttackFront01);
}else if(command.walkV < -50){
// Defensive Posture
bioloid.playSeq(Defence01);
}else if(command.walkH > 50){
// Attack Left
bioloid.playSeq(LeftAttack01);
}else if(command.walkH < -50){
// Attack right
bioloid.playSeq(RightAttack01);
}
}
}
// if we press the left button and not the right button
//then modify the look joystick as follows
if(command.buttons&BUT_R1 == 0) {
if(command.buttons&BUT_L6 > 0) {
if(command.lookH > 50)
// twist to one side.
{
koolx += 5;
SetPosition(21,koolx);
}else if(command.lookH < -50)
// twist to the other side.
{
koolx += -5;
SetPosition(21,koolx);
}
}
}

if(bioloid.playing > 0) {
bioloid.play(); // continue sequence
}
}
}
} //end loop


DB

mannyr7
07-12-2011, 05:44 PM
Hey DB, I'll be tweaking my code eventually to something similar to what you are trying to do. Not wanting to start a religious discussion over code formatting, I tend to write mine this way to better follow nested statements. This is just my personal preference, to each his own. :happy:


// press the first button on the right side
// This turns the joystick into an attack commander.
if(command.buttons&BUT_R1 > 0) //If far right button is pressed, continue...
{
if(command.buttons&BUT_L6 == 0) //and if far left button is not pressed, continue...
//This "if" statement seems unneccesary.
{
if(command.walkV > 50) // and execute Attack sequence depending on walk stick position.
{
// Front attack
bioloid.playSeq(AttackFront01);
}
else if(command.walkV < -50)
{
// Defensive Posture
bioloid.playSeq(Defence01);
}
else if(command.walkH > 50)
{
// Attack Left
bioloid.playSeq(LeftAttack01);
}
else if(command.walkH < -50)
{
// Attack right
bioloid.playSeq(RightAttack01);
}
}
}
// if we press the left button and not the right button
//then modify the look joystick as follows
if(command.buttons&BUT_R1 == 0) // If far right button is not pressed continue...
{ // This one also seems unneccessary.
if(command.buttons&BUT_L6 > 0) // and if far left button is pressed continue...

if(command.lookH > 50) // and twist depending on look stick position.
// twist to one side.
{
koolx += 5;
SetPosition(21,koolx);
}
else if(command.lookH < -50)
// twist to the other side.
{
koolx += -5;
SetPosition(21,koolx);
}
}
}

darkback2
07-12-2011, 07:31 PM
So this may be closer than before, but it still doesn't work.



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

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

// variables for looking
int koolx;
int kooly;

void setup(){
//open the serial port
command.begin(38400);
// stand up slowly
delay(1000); // recommended pause
bioloid.loadPose(up01);
bioloid.readPose();
bioloid.interpolateSetup(500);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
//initialize variables for looking.
koolx = 512;
kooly = 512;
}
}

void loop(){
if(command.ReadMsgs() > 0) // check for new messages
{
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0) // only process if we aren't currently doing a sequence
{
//I am using the left most and right most buttons to modify other
//commands. In order to do this I need to make sure the bottons
//are either pressed or not pressed to create the conditions under which
//commands should be followed.
//If no buttons are pressed then the walk and look joysticks
//move the robot around.

if(command.buttons&BUT_R1 == 0) //make sure button r1 is not pressed.
{
if(command.buttons&BUT_L6 == 0) //make sure button l6 is not pressed.
{
if(command.walkV > 50)
{
bioloid.playSeq(simplewalk01); // walk forward
}
else if(command.walkV < -50)
{
bioloid.playSeq(Rest01);// Crouch Down
}
else if(command.walkH > 50)
{
bioloid.playSeq(JturnL01);// turn left
}
else if(command.walkH < -50)
{
bioloid.playSeq(JturnR01);// turn right
}
else if(command.lookH > 50)
{
bioloid.playSeq(JturnL01);// turn right
}
else if(command.lookH < -50)
{
bioloid.playSeq(JturnR01);// turn right
}
}// close if bottonL6
}// close if button R1

// press the first button on the right side
// This turns the walk joystick into an attack commander.
if(command.buttons&BUT_R1 == BUT_R1)
{
if(command.buttons&BUT_L6 == 0)
{
if(command.walkV > 50)
{
bioloid.playSeq(AttackFront01);// Front attack
}
else if(command.walkV < -50)
{
bioloid.playSeq(Defence01);// Defensive Posture
}
else if(command.walkH > 50)
{
bioloid.playSeq(LeftAttack01);// Attack Left
}
else if(command.walkH < -50)
{
bioloid.playSeq(RightAttack01);// Attack right
}
}
}

// if we press the left button and not the right button
//then modify the look joystick as follows
if(command.buttons&BUT_R1 == 0) {
if(command.buttons&BUT_L6 == BUT_L6) {
if(command.lookH > 50)
{
// twist to one side.
koolx += 5;
SetPosition(21,koolx);
}
else if(command.lookH < -50)
{
// twist to the other side.
koolx += -5;
SetPosition(21,koolx);
}
}
}
}
}

if(bioloid.playing > 0) {
bioloid.play(); // continue sequence
}
} //end loop
Thanks to Fergs and Upgrayd for helping me with this.
DB

kamondelious
07-14-2011, 03:27 PM
This is what fergs was saying to do.



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

BioloidController bioloid = BioloidController(1000000);
Commander command = Commander();

void setup(){

//open the serial port
command.begin(38400);
// stand up slowly
delay(1000); // recommended pause
bioloid.loadPose(up01);
bioloid.readPose();
bioloid.interpolateSetup(500);
while(bioloid.interpolating > 0){
bioloid.interpolateStep();
delay(3);
//variables for looking.

}
}

void loop(){
int koolx;
koolx = 512;
int kooly;
kooly = 512;
if(command.ReadMsgs() > 0){ // check for new messages
digitalWrite(0,HIGH-digitalRead(0)); // toggle LED so we know johnny5 is alive
if(bioloid.playing == 0){ // only process if we aren't currently doing a sequence
//val = (command.buttons&BUT_R1 > 0) ? 1:0;
int rb1 = (command.buttons&BUT_R1 > 0) ? 1:0;
int lb6 = (command.buttons&BUT_L6 > 0) ? 1:0;
// if no buttons are pressed then the walk and look joysticks
//move the robot around.
if(rb1 == 0) {
if(b6 == 0) {
if(command.walkV > 50){
// walk forward
bioloid.playSeq(simplewalk01);
}else if(command.walkV < -50){
// walk Crouch Down
bioloid.playSeq(Defence01);
}else if(command.walkH > 50){
// turn left
bioloid.playSeq(JturnL01);
}else if(command.walkH < -50){
// turn right
bioloid.playSeq(JturnR01);
}else if(command.lookH > 50){
// turn right
bioloid.playSeq(JturnL01);
}else if(command.lookH < -50){
// turn right
bioloid.playSeq(JturnR01);
}
}
}
// press the first button on the right side
// This turns the joystick into an attack commander.
if(rb1 > 0) {
if(lb6 == 0) {
if(command.walkV > 50){
// Front attack
bioloid.playSeq(AttackFront01);
}else if(command.walkV < -50){
// Defensive Posture
bioloid.playSeq(Defence01);
}else if(command.walkH > 50){
// Attack Left
bioloid.playSeq(LeftAttack01);
}else if(command.walkH < -50){
// Attack right
bioloid.playSeq(RightAttack01);
}
}
}
// if we press the left button and not the right button
//then modify the look joystick as follows
if(rb1 == 0) {
if(lb6 > 0) {
if(command.lookH > 50)
// twist to one side.
{
koolx += 5;
SetPosition(21,koolx);
}else if(command.lookH < -50)
// twist to the other side.
{
koolx += -5;
SetPosition(21,koolx);
}
}
}

if(bioloid.playing > 0) {
bioloid.play(); // continue sequence
}
}
}
} //end loop

victory12
11-11-2011, 10:05 PM
OK, thanks. :happy: I was going through the various tutes that exist for PyGame, which seem to have the info of what I need to learn.
I just need to put it all together now. It could take a while.

seriously the real doing here was done by other people. I was really lucky to get all of the help I got from Chris Novak and Fergy. I honestly only typed a few words in putting this together. the cool part is I think its commented well enough that I actually understand what I did.

s for opening a serial port, you can start by using a the software you use to set up your xbees. I'll be putting together a visual program where you can trigger poses or individually move servos in the next few weeks...and more likely than not this weekend.
__________________
virtual assistant (http://globolstaff.com)