PDA

View Full Version : [Question(s)] PhoenixDriver AX12 SetRegOnAllServos2 ?



Zenta
04-03-2016, 03:07 PM
Hi,

I'm currently working on a new gait engine for hexapods, its still a WIP but it works very well so far. I'll make another thread about it when I get time..

Anyway, my question here is simply if anyone (or KurtE) has thought of a SetRegOnAllServos2 function?
I mean a function to set a register on all servos (using sync write) like Torque Limit that require a 16 bit value (L and H). I'll probably try to write it myself but I just wanted to hear if anyone has already written it, because I'm a bit unsure how to write it..:o

KurtEck
04-03-2016, 03:50 PM
Hi Zenta,

Two ways to write it:

Here is code from my Raspberry Pi project:

//--------------------------------------------------------------------
//[SetRegOnAllServos] Function that is called to set the state of one
// register in all of the servos, like Torque on...
//--------------------------------------------------------------------
void SetRegOnAllServos(uint8_t bReg, uint8_t bVal)
{
dxl_set_txpacket_id(AX_ID_BROADCAST);
dxl_set_txpacket_instruction(AX_CMD_SYNC_WRITE);

dxl_set_txpacket_parameter(0, bReg); // which register
dxl_set_txpacket_parameter(1, 1); // 1 byte
dxl_set_txpacket_length(2*NUMSERVOS+4);

for (byte i = 0; i < NUMSERVOS; i++) {
dxl_set_txpacket_parameter(2+i*2, (cPinTable[i])); // 1 byte
dxl_set_txpacket_parameter(3+i*2, bVal); // 1 byte
}
dxl_txrx_packet();
//return dxl_get_result(); // don't care for now
}


But again this is setup with the Linux stuff...

From the Arduino Phoenix in parts project I have:

//--------------------------------------------------------------------
//[SetRegOnAllServos] Function that is called to set the state of one
// register in all of the servos, like Torque on...
//--------------------------------------------------------------------
void SetRegOnAllServos(uint8_t bReg, uint8_t bVal)
{
// Need to first output the header for the Sync Write
int length = 4 + (NUMSERVOS * 2); // 2 = id + val
int checksum = 254 + length + AX_SYNC_WRITE + 1 + bReg;
setTXall();
ax12write(0xFF);
ax12write(0xFF);
ax12write(0xFE);
ax12write(length);
ax12write(AX_SYNC_WRITE);
ax12write(bReg);
ax12write(1); // number of bytes per servo (plus the ID...)
for (int i = 0; i < NUMSERVOS; i++) {
byte id = pgm_read_byte(&cPinTable[i]);
checksum += id + bVal;
ax12write(id);
ax12write(bVal);

}
ax12write(0xff - (checksum % 256));
setRX(0);
}


The other option is to do like what Kevin did with ROS version, where you simply send a broadcast:
Example in the hexapod_ros (servo_driver.cpp), to make sure that all servos are on the code does:

dxl_write_word( 254, TORQUE_ENABLE, 1 );
Which is a lot simpler. However it does have the side effect if you have other servos on your robot, example maybe an ARM, the servos associated with the Arm would also be enabled.

(Also note to self) - should change dxl_write_word to dxl_write_byte as TORQUE_ENABLE is a one byte register. The current code is writing a 1 to TORQUE_ENABLE and a 0 to LED...

Kurt

Zenta
04-03-2016, 04:58 PM
Thanks Kurt,

I'm aware of the SetRegOnAllServos function in your Phoenix parts code. My point was how to write this for a register with high/low byte. That's why I entitled the thread SetRegOnAllServos2.

It's getting later here in Norway, so I might have to look more into it another day.

I assume the value must be a word type, say word wVal ?. And for dividing the Low and High byte I use the code ax12write(wVal&0xff); ax12write(wVal>>8);. I also assume the int length = 4 + (NUMSERVOS * 3); where 3 is id + val high and low.

Is that correct?

KurtEck
04-03-2016, 06:20 PM
Oops, I missed the two byte stuff.. Could be something like:

//--------------------------------------------------------------------
//[SetRegOnAllServos2] Function that is called to set the state of one
// register in all of the servos, like Torque on...
//--------------------------------------------------------------------
void SetRegOnAllServos2(uint8_t bReg, uint16_t wVal)
{
// Need to first output the header for the Sync Write
int length = 4 + (NUMSERVOS * 3); // 3 = id + val.low val.high
int checksum = 254 + length + AX_SYNC_WRITE + 2 + bReg;
uint8_t low_byte = wVal & 0xff;
uint8_t high_byte = (wVal >> 8) & 0xff;
setTXall();
ax12write(0xFF);
ax12write(0xFF);
ax12write(0xFE);
ax12write(length);
ax12write(AX_SYNC_WRITE);
ax12write(bReg);
ax12write(2); // number of bytes per servo (plus the ID...)
for (int i = 0; i < NUMSERVOS; i++) {
byte id = pgm_read_byte(&cPinTable[i]);
checksum += id + low_byte + high_byte;
ax12write(id);
ax12write(low_byte);
ax12write(high_byte);
}
ax12write(0xff - (checksum % 256));
setRX(0);
}
Note: I updated this from the other example on the fly so there may be issues.

Kurt

Zenta
04-04-2016, 04:00 PM
Awesome, thanks for your help Kurt! It worked very well.:veryhappy:
I see now that I missed some crucial points like the +2 on checksum.
Currently I'm using this on a simple startup routine setting the torque very low at first on all servos. I'll then check if all servos has reached their init positions (simply check if each servo has stopped moving by reading the speed I think?). Then I'll set the torque at full again. I don't like the legs snapping fast into position at startup. Some stuff (or fingers) might get hurt one time..

Thanks!

Zenta
04-10-2016, 05:14 PM
Hi,

I did some coding this evening and I noticed something strange (or maybe it isn't?).

For some reason I needed to call the enable torque function before changing the torque limit.

Example, if I call this line of code alone:

SetRegOnAllServos2(AX_TORQUE_LIMIT_L, 1023);//Turn on full Torque
Randomly some of the servos still keeps the previous torque settings.

But if I call the enable torque first, like this:

SetRegOnAllServos(AX_TORQUE_ENABLE, 1); // Use sync write to do it.
SetRegOnAllServos2(AX_TORQUE_LIMIT_L, 1023);//Turn on full Torque

All servos are set to the new torque value, without any random fault.

Does anyone have any idea of why it's like this?

(FYI, the torque was enabled on all servos at first, of course.)

KurtEck
04-11-2016, 09:53 AM
Sorry, no idea.

Is it consistent? i.e. the same servos? If so maybe check to see if by chance they have a different version of firmware than the others.

But again I am just throwing a dart...

Zenta
04-11-2016, 10:02 AM
Thanks for your reply Kurt!

Sorry, no idea.

Is it consistent? i.e. the same servos? If so maybe check to see if by chance they have a different version of firmware than the others.

No, it seem random on 2 - 4 servos that keep the old torque value.

I'm not that worried though, since I kinda found a solution for it. I'm just curious if this is normal? Maybe I should have a logic analyzer to check what data is sent to the servos..

KevinO
04-11-2016, 11:33 AM
I've had the same issue simply asking for their current positions. I ended up turning torque on first as well and that seemed to fix the issue for me.

Zenta
04-11-2016, 03:05 PM
Thanks Kevin, that's kinda useful confirmation.. At least I'm not alone. But how did you end up turning on torque, just a coincidence?

KurtEck
04-11-2016, 03:25 PM
But how did you end up turning on torque, just a coincidence?
In his ROS code he has a function: makeSureServosAreOn
where he:

dxl_write_word( 254, TORQUE_ENABLE, 1 );
i.e. he does a broadcast to all servos, and then he iterates over his servos and reads their current position.

Versus my RPI code function MakeSureServosAreOn where I do a Sync write - SetRegOnAllServos(AX_TORQUE_ENABLE, 1), to enable the torque on the servos that are specifically part of the hexapod...

KevinO
04-11-2016, 03:55 PM
Even to complicate things I noticed I had to sleep for a brief moment after turning on torque. If I didn't I didn't get a proper current position from the first few servos. Could be a Linux issue though.

jwatte
04-12-2016, 10:46 AM
I don't think that's a Linux issue. If it was Linux, you couldn't get a proper command out on the serial bus at all.

KurtEck
04-12-2016, 11:31 AM
Maybe we should look at it with Logic Analyzer and/or Scope, to see anything.

I doubt there would be much of a voltage drop with you turning on your 24 MX servos, as you have not told them to move.

Issue specific to USB2AX? Probably not in Zenta's case as I believe he is still using a Teensy?

Maybe issue on the broadcast ID? Probably not... But is the behavior different if instead of broadcast message, you use SYNC_WRITE?

Likewise is it different if you output separate enable commands to each servo. However in this case, you would need to wait for servo to respond...

Zenta
04-17-2016, 04:54 PM
I think I'm going crazy..


But if I call the enable torque first, like this:

SetRegOnAllServos(AX_TORQUE_ENABLE, 1); // Use sync write to do it.
SetRegOnAllServos2(AX_TORQUE_LIMIT_L, 1023);//Turn on full Torque

All servos are set to the new torque value, without any random fault.

Wrong. Today I got the same error again. Even when enabling torque at first. Kevin mentioned he had to sleep a brief moment before reading positions, for how long?

I've tried to figure out whats going on. What I noticed is that the error occur mostly on some of the tibias (LF,LM and RR). Maybe its a power issue, since they are the last servo on each cable chain? I've never noticed the bug on the coxas (so far). And in some rare cases the femurs. Instead of using the SetRegOnAllServos functions (sync write) I tried a simpler way by just using ax12SetRegister functions and the 254 id for broadcasting all servos. I feel that helped a little.. But still some cases of faults. I'm not sure if it's just random but I get the error a little more often at first power-up after a few minutes of power-off. Since the fault occur mostly on the same servos, I think I'll replace the wires. I think I have some servo wires with a black connector I'm not sure if they are better (higher gauge?).

Pretty frustrated at the moment. lol :(

Edit; yes Kurt, I'm still using the Teensy.

KevinO
04-17-2016, 08:37 PM
Slightly off topic but why do you have to set the torque limit? I'll be honest I've never touched that since it should be full by default correct?

My issue when it happens is always a rear coxa doesn't report its current position. So I totally feel your frustration!

KevinO
04-17-2016, 08:38 PM
And to answer your question it was a half a second before reading.

Zenta
04-18-2016, 01:36 AM
Slightly off topic but why do you have to set the torque limit? I'll be honest I've never touched that since it should be full by default correct?

My issue when it happens is always a rear coxa doesn't report its current position. So I totally feel your frustration!
Thanks for your reply Kevin. Yeah, I hate random errors..

I did post an answer to your question at page 1:

Mostly for safety.


Currently I'm using this on a simple startup routine setting the torque very low at first on all servos. I'll then check if all servos has reached their init positions (simply check if each servo has stopped moving by reading the speed I think?). Then I'll set the torque at full again. I don't like the legs snapping fast into position at startup. Some stuff (or fingers) might get hurt one time..

I could use another method, like reading the current position and do a FK. But that might be a problem if the legs are powered on while one leg is in an illegal position. I also think its a lot safer to start with a very low torque at first in case some of my kids accidentally turn on my robot while holding the legs or something.

And to answer your question it was a half a second before reading.
Oh! That's half a year. lol. I'll test it later today.

KurtEck
04-18-2016, 08:54 AM
Hi Kåre,

A couple of ideas:

a) Update Teensy to output AX at 5vs - Probably not issue as seen similar issues with USB2AX... Alternatively could try Elmue's setup - again doubt will help, would need to short RX/TX

b) Just make the first move real real slow, like maybe a second or two.

c) Do the FK stuff you mentioned. You could then make decisions, like: all legs are near normal startup so do quick startup. Or all legs are in reasonable positions, so do slow startup. Or one (or more) legs are in bad position, but can do some intermediate move to get that leg(s) into better position to then do startup. Or they are to far out of position, so bail and tell user to fix it...

Long time ago, I did a subset of this when I was playing around with Arm code. When you first powered up, if the arm was in some poor position, it would bang parts of the arm right into base and at times hang up. So added an intermediate startup position, which was easier to get to in most cases and then move to startup place. With Lynxmotion arm I always did this as no way to know where servos were, but with AX servos I think I may have added quick test to see if I needed to do this or not...

Personally I think it would look pretty cool if for example one leg is tucked under body, if the hex would use other legs to lift up, then pull it's leg out and then settle down into starting position.

But, back to why reading position does not work (all of time). Would be good to see how it is failing. Like hook up LA (or scope) to one of the servos which fails. What is the voltage of the signal? Is it different when it fails than when it succeeds? Is it receiving a valid packet for the request? Does it send out any response? If so, is it timing out? Or is it outputting the start of the response, than delay and then tries to finish rest of response and timed out there? This might be more relevant in Kevin's case as you are sending out packets to the individual servos.

Maybe not so much in Kåre's case as he is doing two SYNC_WRITE messages, so no response packets to timeout. More likely servo is busy from first SYNC_WRITE? Does changing the order of the servos in the SYNC_WRITE change the behavior? Still might be good to see what the packet looks like at that servo when it fails.

What happens if you don't use Sync write but instead send packet to each servo? If you check the response does it come back as valid? Or timeout? ...

Zenta
04-18-2016, 12:15 PM
Thanks for your input Kurt!


a) Update Teensy to output AX at 5vs
Not sure how I do that, any hint?


b) Just make the first move real real slow, like maybe a second or two.
Yes, I did something like that at first. But the torque is still on full at startup and the servos moves relative fast if they are very far from the init positions.

c) Do the FK stuff you mentioned. You could then make decisions, like: all legs are near normal startup so do quick startup. Or all legs are in reasonable positions, so do slow startup. Or one (or more) legs are in bad position, but can do some intermediate move to get that leg(s) into better position to then do startup. Or they are to far out of position, so bail and tell user to fix it...
Good idea. I might do something like that at the end.


Personally I think it would look pretty cool if for example one leg is tucked under body, if the hex would use other legs to lift up, then pull it's leg out and then settle down into starting position.
Yeah that would be really cool.



What happens if you don't use Sync write but instead send packet to each servo? If you check the response does it come back as valid? Or timeout? ...
Well, currently I'm not using sync write. I tried a single broadcast instead. The result was more or less the same. But I could try to read the torque value on each servo to verify if the value has changed. Then do another attempt to fix it..

KurtEck
04-18-2016, 12:49 PM
Not sure how I do that, any hint?
Well I keep meaning to Order some updated boards :lol:
65456546

Although before I do, I will probably edit the Reset pin size and verify location, such that pogo pin would work. Also change Title from 3.1 to 3.2... Plus who knows maybe update a few things like VR...

Kurt

Note: this setup uses both TX/RX with pin 2 as direction pin, so allows the standard Teensy Usart Interrupt handler can automatically switch from TX to RX when the last TX byte is output.

Zenta
04-18-2016, 04:18 PM
Well I keep meaning to Order some updated boards :lol:


Ok, I see. Not sure if I'll go that route yet.
At first I replaced some servo wires and only broadcasting torque enable and torque limit. Not sure what, but I didn't see the bug very often after that. I then tried waiting 500mS before setting the torque limit and that seem to work ok so far..

Zenta
04-19-2016, 04:39 PM
Hi,

I'm trying to read the current position using this code:

for (LegIndex = 0; LegIndex < CNT_LEGS; LegIndex++) {
CurrentCoxaPos = ax12GetRegister(pgm_read_byte(&cPinTable[FIRSTCOXAPIN + LegIndex]), AX_PRESENT_POSITION_L, 2);
CurrentFemurPos = ax12GetRegister(pgm_read_byte(&cPinTable[FIRSTFEMURPIN + LegIndex]), AX_PRESENT_POSITION_L, 2);
CurrentTibiaPos = ax12GetRegister(pgm_read_byte(&cPinTable[FIRSTTIBIAPIN + LegIndex]), AX_PRESENT_POSITION_L, 2);
DBGSerial.print(CurrentCoxaPos, DEC);
DBGSerial.print(" ");
DBGSerial.print(CurrentFemurPos, DEC);
DBGSerial.print(" ");
DBGSerial.print(CurrentTibiaPos, DEC);
DBGSerial.print(" _ ");

delay(25);
}

For some reason I can't get the correct position data from two of the servos. The result is very consistent. May I have a couple of faulty servos?

Example of output:

340 798 967 _ 509 808 974 _ 510 804 975 _ 609 459 4294967295 _ 4294967295 205 47 _ 509 214 46 _
437 813 978 _ 509 821 983 _ 509 817 982 _ 509 272 4294967295 _ 4294967295 191 41 _ 510 199 41 _
468 813 979 _ 509 821 982 _ 509 818 982 _ 509 236 4294967295 _ 4294967295 190 41 _ 510 197 41 _
501 815 979 _ 509 822 982 _ 509 819 982 _ 509 202 4294967295 _ 4294967295 190 40 _ 510 197 41 _
510 817 982 _ 509 823 982 _ 509 819 982 _ 509 201 4294967295 _ 4294967295 191 41 _ 510 196 41 _
510 818 983 _ 509 823 982 _ 509 821 982 _ 509 200 4294967295 _ 4294967295 190 40 _ 510 195 40 _
510 818 982 _ 509 823 982 _ 509 821 983 _ 509 198 4294967295 _ 4294967295 190 40 _ 510 195 40 _
510 821 982 _ 509 823 982 _ 509 822 982 _ 509 197 4294967295 _ 4294967295 190 40 _ 510 195 40 _
510 822 982 _ 509 824 982 _ 509 822 982 _ 509 196 4294967295 _ 4294967295 191 40 _ 510 194 40 _

Did try enable torque at first and then a 500mS but that didn't help.
I might try to replace one of the servos to see if thats the problem.

Any ideas or tips are appreciated.

KurtEck
04-19-2016, 05:12 PM
Hi Kåre, My guess from this is AX servos as coxa's are near 512.

Don't know for sure which servo numbers you have here, but if looking at my Phantom_Phoenix sketch I might guess 11 and 13?

If it were me, I would probably load my AX12 test program (https://github.com/KurtE/AX12_Test). Not because it is great, but because I am used to it. I would run the servo scan (I think command 4) and see what servos it complains about. Warning, this program is probably configured to not use servo #1, but renumbered 1 to 19...

Again warning, code is just thrown together at different times, so nothing special.

If they are not found, I have a tendency to see if by chance they are now servo #1. I often do this by
using the set Servo position command(2): Something like: 2 1 600
Which says move servo 1 to position 600. If multiple servos move, you got hit by the servo number reset problem.
Can then use command 8 to reset... Something like: 8 1 11
To renumber all servos #1's to servo 11. So unplug other ones that are number 1...

If it sometimes finds these servos, I often also try to see if I can read information about the servo, like: 9 11
To print out stuff about servo 11... I check to see if for example the delay time is set high (i.e. not 0)...
Also have another command to set the delay times to 0. I think if you don't specify which servo it loops through all of the logical ones for the robot...

But again I am just guessing

KevinO
04-19-2016, 05:29 PM
I had the same thing on two of my MX servos. The buffer chip failed apparently. I had to get them repaired. It was a consistent value no matter what physical position they were in.

KevinO
04-19-2016, 05:31 PM
I should mention they listened to any command and performed well. Just didn't respond to certain queries.

Zenta
04-20-2016, 12:25 AM
Thanks for your input Kurt and Kevin!

Hi Kåre, My guess from this is AX servos as coxa's are near 512.
Don't know for sure which servo numbers you have here, but if looking at my Phantom_Phoenix sketch I might guess 11 and 13?

That's correct, one tibia and one coxa servo. I'm still working on the code only, so I've not made any new robots yet.
I doubt it's an id#1 issue. But I'll do some tests on the two servos. I do have one spare AX-18A, so I might have to live with one faulty servo and just ignore readings >1023.

I should mention they listened to any command and performed well. Just didn't respond to certain queries.
It sounds like that's the case, since it perform ok. But if I remove the 25mS delay I get some random reading errors from others servos too. I believe I read somewhere there had to be a delay. Strange, since I'm only using the delay for every third readings.

Zenta
04-20-2016, 02:38 PM
Hi,

My two servos ID 11 and 13 seem to be faulty. I'm using the Robotis Roboplus Manager and both servos is detected as an unknown device:
6552

Instead of a normal servo:
6553

I'm surprised they work at all, lol. Looks like it might be the same issue Kevin had on his two faulty MX-64?

KurtEck
04-20-2016, 03:13 PM
Have you tried a firmware recovery on these? I had a servo that stopped responding and that took care of it (I think).

Zenta
04-20-2016, 03:20 PM
Have you tried a firmware recovery on these? I had a servo that stopped responding and that took care of it (I think).
I'm not sure if I have the tool for that. I've an old CM-5 and I think I need a USB2Dynamixel to get access to their Dynamixel manager. I do have a USB2AX though.

Or is there another way to update the firmware on the servo?

KurtEck
04-20-2016, 03:22 PM
I believe I was able to do it with USB2AX, again it has been awhile since I tried.

tician
04-20-2016, 03:40 PM
Servo firmware update is done by connecting only a single dynamixel to a USB2Dynamixel or USB2AX and using the Dynamixel Wizard/Manager/whatever firmware update. It will prompt to connect only a single servo then restart the servo by disconnecting and reconnecting power, then it should list the servo model found and give you options for updating the firmware.

Zenta
04-20-2016, 05:28 PM
Thanks for your help and input Kurt, Kevin and Tician!

It worked! Both servos are now recovered with new firmware and reading positions works fine! :D

I've just finished the "WakeUp" code. At power-up the legs slowly and safely move into a init position with all legs 10mm above ground. Then it "wake up" and turn on full torque and lower the legs down to ground again. I've also added a bell/warning that start beeping if the legs hasn't found their positions within 6 seconds. Actually, that part worked fine in my small workshop when one of the legs got hooked up into stuff on my desk. So instead of total mayhem the bot only made small cute beeps until I released the leg. LOL. So cool.

Thanks and good night!

r3n33
04-22-2016, 11:18 AM
I've just finished the "WakeUp" code. At power-up the legs slowly and safely move into a init position with all legs 10mm above ground. Then it "wake up" and turn on full torque and lower the legs down to ground again. I've also added a bell/warning that start beeping if the legs hasn't found their positions within 6 seconds. Actually, that part worked fine in my small workshop when one of the legs got hooked up into stuff on my desk. So instead of total mayhem the bot only made small cute beeps until I released the leg. LOL. So cool.

Smart! A great idea.

Also the anticipation of what all you are putting together is growing :) Can't wait to see more of your new gait stuff as well.

Zenta
04-22-2016, 01:23 PM
Smart! A great idea.

Also the anticipation of what all you are putting together is growing :) Can't wait to see more of your new gait stuff as well.
Thanks R3n33!:D
I've just posted 3 new short teaser clips on my Instagram.:p

KurtEck
04-22-2016, 01:39 PM
Looks Great!