AX-12 control with PIC32

[i-Bot]

Hi, pleased to see you are persevering with this.

I don't use PIC32, so can't give complete answer, but it is pretty clear your Dynamixel receive routine will not work as you expect.

If I consider what is happening at the point in your code after you have sent out the PING command and set the buffer direction to input.

At this time the Dynamixel will process the PING command and generate the response. After the return delay time set in the servo (default = 500microsec) it will send out the response.
The first character of the response will be available in the PIC rx buffer after 500microsec plus the serial time (10microsec). The following characters will be available each 10 microsec after that.

This means that when you do the(DataRdyUART()) no data has yet arrived, so it should sail straight through the "while" statement.

I don't know why you get 1 character, it may be that you have a character in the receive buffer due to the startup. It is always best to clear the receive buffer on startup. Do you have a 10K pullup on the AX bus ? Going through the loop a second time is more confusing, because you will then have the response from the previous PING in the UART receive buffer.

A quick and dirty fix would be to add 1 millisec delay after the direction change to give time for complete reply to enter the UART receive buffer before you do the "while(DataRdyUART()) ".

A more robust receive routine should check and allow for errors in response, also have some timeout in case a response does not come.

[ColdStart]

i-Bot, thank you for reply! Well... yes i do have 10k pullup on AX12 data bus.

Here i am attaching the diagram iv drawn from the scope, when i hooked its channel to the TX line from PIC32.

here is link to picture:
http://picpaste.com/pics/scope1-zkij5DNe.1305168983.gif

Again the sequence i am transmitting is: 0xff 0xff 0x01 0x02 0x01 0xfb

You can notice some features...but at the end it doesnt look much like 0xfb isnt it? However, any other command (like turn or set LED On) is being accepted by AX12 and it executes it.


For some reason i cannot capture something on the RX line... right now im working on it... but what can you say about this diagram? is it something what is supposed to look like for transmission of my sequence?

[ColdStart]

ok after few more hours of debugging/checking i found something.

According to pahe 7 of AX-12 manual, when DIR is 1, the TX buffer is activated and you send the commands to motor, however when DIR is 0, the TX is now disabled, however the RX buffer is enabled, because 0 is being passed through 74HC04 and becomes 1, this way enabling RX buffer. That is how it must work.


in my code i did set DIR to 1 and then send commands to motor, and motor understands all those commands.


Now! Interesting thing is that when PIC32 sets DIR to 0, and that 0 passes through 74HC04 it DOES NOT BECOME 1 !

On the PIC32 pin it is 0, on the input of 74HC04 it is also 0, but on the 2nd output of it, it is still 0!!

Of course all chips as well as motor have common ground. I checked the connections.

Also, i decided to test my 74HC04 with a separate battery circuit, and it performed well (it did turn 0 into 1).

But in my case, when i hook that converter circuit to PIC32 from some reason it does not invert the signal from PIC32 into 1...

I guess this is the major problem right now...because RX buffer is simply not activated...


Any ideas now?

[ColdStart]

OK, i think there is some progress...first of all my DIR connection assignment in PIC32 was wrong (it is really weird how come it transmitted data correctly without setting DIR to 1.... that is why it took so much time to debug, because i was thinking that since i can transmit that is correctly...unless i decided to go from scratch everywhere).


Now.. here is my updated code... basically...what i am doing is, i am sending command to AX12 with button1 press... then i give it fairly enough time to return data, and then upon pressing of button2 i start reading RX registers in PIC32....

I also hooked up scope to RX line...and i can see the response of AX12 after approximately 932us.


here is a code:

Code: Select all

while(1)
{

   if (Button1Pressed)
   {
      LED2On();



      
   // obtain status
     DIR1();
        putcUART1(0xff);
        putcUART1(0xff);
        putcUART1(0x01);
        putcUART1(0x02);
        putcUART1(0x01);
        putcUART1(0xfb);
        while(BusyUART1());
     DIR0();


      putsUART2("Command 1 is sent.\r\n");

      
      while (Button1Pressed) ; // wait until depressed


   } else
      LED2Off();



   if (Button2Pressed)
   {
      LED2On();
 

  //while(DataRdyUART1())
  for(i=0; i<20; i++)
  {
      data[i] = getcUART1();
     // di+=1;
  }
   
   
      // writing the results on SD card
   //   int i=0;
      for(i=0; i<20; i++)
      {
         FSfwrite (&data[i], 1, 1, pointer);
      }
   
      FSfwrite (&di, 1, 1, pointer);
   
            FSfclose(pointer);




      putsUART2("Command 2 is sent.\r\n");

      
      while (Button2Pressed) ; // wait until depressed

   } else
      LED2Off();


}

Now... i read my recorded data from SD card and see the following:

ff ff 01 02 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00


some kind of success isnt it... but....

my response was supposed to be ff ff 01 02 00 fc

so now i do not see fc for some reason.... strange... ill look more of course too.

but would like to see your opinion?

because it seems like if i already can read it all out and have almost all correct data...then why i dont get fc...

?

[i-Bot]

The receive buffer on the PIC32 UART is only 4 bytes long. The 5th byte may be from the UART receive buffer, or it may just be 0 because it is not overwritten.

If you want all the response bytes, you will need a better receive routine.

[ColdStart]

thank you.

Yes i looked at datasheet also, it says its 8 bytes long for some other devices, but for mine its 4 as you said.

What i did is just decided to do it RX interrupt, in order to catch it. And it worked.

here is a code for interrupt handler:

Code: Select all

////////////////////////////////////////////////////////////////////
// UART 2 interrupt handler
// it is set at priority level 2
void __ISR(_UART1_VECTOR, ipl2) IntUart2Handler(void)
{
   // Is this an RX interrupt?
   if(mU1RXGetIntFlag())
   {
      // Clear the RX interrupt Flag
       mU1RXClearIntFlag();

      // Echo what we just received.
      //putcUART1(ReadUART1());
      data[di] = getcUART1();
      di+=1;

      // Toggle LED to indicate UART activity
      //mPORTAToggleBits(BIT_7);

   }

   // We don't care about TX interrupt
   if ( mU1TXGetIntFlag() )
   {
      mU1TXClearIntFlag();
   }
}
////////////////////////////////////////////////////////////////////


and here is code for main infinite loop:

Code: Select all


// Infinite loop
while(1)
{

   if (Button1Pressed)
   {
      LED2On();



      
   // obtain status
     DIR1();
        putcUART1(0xff);
        putcUART1(0xff);
        putcUART1(0x01);
        putcUART1(0x02);
        putcUART1(0x01);
        putcUART1(0xfb);
        while(BusyUART1());
     DIR0();


   // Enable RX interrupts for UART1
   ConfigIntUART1(UART_INT_PR2 | UART_RX_INT_EN);


      putsUART2("Command 1 is sent.\r\n");

      
      while (Button1Pressed) ; // wait until depressed


   } else
      LED2Off();



   if (Button2Pressed)
   {
      LED2On(); 
   
      // writing the results on SD card
   
      for(i=0; i<di; i++)
      {
         FSfwrite (&data[i], 1, 1, pointer);
      }
   

   
            FSfclose(pointer);




      putsUART2("Command 2 is sent.\r\n");

      
      while (Button2Pressed) ; // wait until depressed

   } else
      LED2Off();


}

So during an interrupt, the interrupt handling routine basically starts filling an array defined globally as unsigned char data[20], then upon pressing of button 2 the function just starts saving the data from that array on SD card (someone else might do something what he prefers)


For now i think im done, thanks all for discussion, especially i-Bot

If some other stuff/issues pops out later i will post here again. I think PIC32 is pretty popular devices and it would be nice to fully integrate it with different servos.

[ColdStart]

one more thing i noticed, for those who want to replicate schematic on page 7 of AX12 manual, you should add one more 10k pullup on the RX line - the input to PIC32, because without that sometimes you read garbage value once the direction is changed to input.


will keep updated.

[ColdStart]

ok i have new problems now

1. i have configured my ax12 to give status packet only to READ commands.

2. i have configured the delay time to be 4us, 2 * value in address 0x05, which is 2.


Next i am collecting received data of 8 bytes, from position to temperature using the following interrupt service routine:

Code: Select all

void __ISR(_UART1_VECTOR, ipl2) IntUart1Handler(void)
{
//   Status1();
   // Is this an RX interrupt?
   if(mU1RXGetIntFlag())
   {
      // Clear the RX interrupt Flag
       mU1RXClearIntFlag();

      data[di] = getcUART1();
      di+=1;

   }

   // We don't care about TX interrupt
   if ( mU1TXGetIntFlag() )
   {
      mU1TXClearIntFlag();
   }

//   Status0();
}

Next, i have another timer interrupt every 10ms, and when that interrupt completes its job, which is indicated by int_done flag, then in my main infinite loop i start moving the ax12 in 4 different positions, back and forth, and also collecting the data. here is a code from pastebin, for some reason here this piece of code does not show up properly:
http://pastebin.com/XHYKER8b

data is declared as unsigned char data[14], di is just index to that data.

What happens is, my motor suddenly stops... and also, when i read the values which it had already written so far, i notice that not far away from a stop...sometimes before stop, sometimes after... it gives me garbage values for position and speed, in other words values are big, more than they should be according to manual.


I tried to see UART1 status error bits, but they were not showing up.


Also, when this thing happens, i tried also putting additional controls inside main loop and start moving it by pressing buttons... it did work! BUT... after few moves from manual button commands, sometimes ax12 stopped with blinking LED.


I want to know at least where could be the error?

WHy do i receive data normally which seems correct, and suddenly after 3-4 minutes of constant operation ax12 stops?

any ideas?

thanks!

also here is readmotor() and moveto0() functions (other move functions are similar)

Code: Select all


void readmotor(void)
{
   
     DIR1();
       putcUART1(0xff); // pakcet TAG
       putcUART1(0xff); // packet TAG
       putcUART1(0x01); // ID
       putcUART1(0x04); // LENGTH
       putcUART1(0x02); // READ
       putcUART1(0x24); // Start address of Present Position L
       putcUART1(0x08); // 8 bytes from start
       putcUART1(0xcc); // Checksum
           while(BusyUART1());
        DIR0();
      
}


void moveto0(void)
{
     DIR1();
    putcUART1(0xff); // pakcet TAG
    putcUART1(0xff); // packet TAG
    putcUART1(0x01); // ID
    putcUART1(0x07); // LENGTH
    putcUART1(0x03); // WRITE
    putcUART1(0x1e); // Start address of Goal Position L
    putcUART1(0x00); // 1st byte, Goal Position L
    putcUART1(0x00); // 2nd byte, Goal Position H
    putcUART1(0x00); // 3rd byte, Moving speed L
    putcUART1(0x02); // 4th byte, Moving speed H
    putcUART1(0xd4); // Checksum
        while(BusyUART1());
     DIR0();   
   
}
 

thank you for any ideas[/url]

[i-Bot]

A couple of pointers to how to resolve this.

" i have configured the delay time to be 4us, 2 * value in address 0x05, which is 2. "

This is a good setting for you, since you are ready to receive data very quickly. The higher settings are for where the controller may be too busy to immediately change the buffer direction. This does not mean that the response will come back in 4 us. If you look here:

http://robosavvy.com/forum/viewtopic.php?t=6616
You will see that minimum is actually around 30us and sometimes as high as 80us. I don't think this is your actual problem since you have a 10ms timer on the read, but be carefull if you reduce the timer period.

" i have configured my ax12 to give status packet only to READ commands."

This is not usually done, the returned status tells you if the command was received and initiated correctly. Also it tells you when the AX12 can receive another command. It is fair to assume that the AX12 is not able to accept another command during the return delay time (30 to 80us) though.
I think you may start the read too soon after the write. A quick and dirty would be to add the 10ms delay after the write too.

Also consider your switch may bounce and give multiple triggers while pressed. This may be masked when you have a long print routine in the loop, but will be an issue if you remove that.

The checksum and status bytes are there for good reason. If there are errors, then these must be attended to first and data discarded or resent as appropriate. Does the bad read data have a good checksum and status ?

If you look at the source code in the USB2Dynamixel SDK, you will see how a more robust program handles this.

[ColdStart]

10ms delay after write? thats too much for my application actually...

i added some dummy for loop between move commands and read command, and this time it lasted some longer, but again after 5 minutes got stuck.

the reason why i made it reply only to READ commands is not to add additional overhead to program, since even without that it is doing so much things.

and yes you are right, even tho i set delay to 4us, it still makes maybe 100us or so sometimes more sometimes less.

yes it seems like the reason might be that at some period of time there is overlap,...also im receiving 14 bytes... thats minimum 140us...

one more thing, 10ms timer is not after the read. Here is what i have:


I have highest priority interrupt timer, it is active every 10ms, when it is activated, it does some another application specific job, which might sometimes last up to 8ms... and sometimes can complete within 200us or less!! when that job is done it sets int_done = 1.

after that, control comes back to main loop, where i start giving movement command and read command... so now i included small loop between read and write in main control loop...and it lasted for 5 minutes.

i will keep trying to figure out the right delays, as well as look at the standard code. and keep updated.

thatnks for hints. will let you know soon.

[PedroR]

Hi ColdStart

I've been following your posts (not too indepth though as I am not familiar with PIC) but I have noticed you are now discussing timing issues between queries and replies of AX servos.

Allow me to jump in and share some information we have found.

While developping the ActuatedCharacter project ( http://actuated.wordpress.com ) the engineers there used an osciloscope to analyse timings on AX-12 servos.

They found that the time it takes for an AX-12 to respond to a command is not fixed and varies randmly (with a significant variantion rate).

They haven't really found out why it happens but it seems related to other internal functions of the servo. The servos receive a command/request and depending on who-knows-what they're doing they sometimes reply immediatelly, other times have a considrable delay before replying.

So if you're basing your code on a fixed time-scale between queries and replies from AX servos, please keep this in mind.


If you do require precise timings and a compressed/efficient protocol, you may want to have a look at the custom AX-12 firmware that was designed for the ActuatedCharacter project (see the link to the blog above).
The firmware was designed with 2 features in mind: efficient protocol transmission, fixed timings for query and reply and they've added precise PWM control (but stripped down some other features).
I don't know if it's of interest to your prject but in case, I leave the information for your reference.


Regards
Pedro.

[i-Bot]

When it fails is the servo LED lit or flashing. You should be able to read the servo status to find out what is wrong.

[ColdStart]

Thank you Pedro, i noticed those delays too with scope, for example once for a read packet of 8 bytes it took around more than 100ms!!! to come back! That is why probably i-Bot recommended read status every time.

But here is a question then, if i have a routine which checks if the status packet from move command received, and lets say i have some delay time (if its not received within 1ms)... then... i do what?

what should i do if i have not received any status packet after sending move command to ax12? because its not gona make any difference...if i will after delay passed time just give another command...it might stop! just as it does now! you see the point? it does not stop now with blinking, it just stops.... however, interesting thing is, even if it stops later it still responds to the data read commands... its just for some reason it stops acting to move commands for my code...

any suggestions for that?


Also, here is a nother thing, i noticed that sometimes i receive current speed and load values as 1464 and 1200... however from table on page 18 the max values for those goal positions (probably as well as current ones) is 1023... and at the same time, error byte was 00, means no error. that is kind of strange too. because all other values seem ok, and correspond to the correct servo positions.

thank you. im modifying code now further. will let you know soon.

[garycmartin]

Also, here is a nother thing, i noticed that sometimes i receive current speed and load values as 1464 and 1200... however from table on page 18 the max values for those goal positions (probably as well as current ones) is 1023... and at the same time, error byte was 00, means no error. that is kind of strange too. because all other values seem ok, and correspond to the correct servo positions.

I don't know about the speed values, but for load values in RoboPlus Task/Manager I found I needed some logic like if load >= 1024 then load = 1024 - load. This gives a +/- anti-clockwise/clockwise range with 0 as no load (and I assume -1023 and +1023 as the limits, though I think I've only seen load values in the low +/- hundreds).

--Gary

[PedroR]

Hi ColdStart

For a number of commands you probably don't need to be parsing the results that you're getting back.

You probably just need to ensure that you receive all the Bytes sent from the servo and empty the incoming buffer.

Probably the servo expects you to receive its reply but you don't need to be waiting to parse it.

I'd just have some interrupt or timer to be emptying the incoming buffer regularly.
And for instructions that you do need to get feedback have a longer wait time and keep waiting (block the interrupt that empties the buffer for example).

The servos are normally quiet so they don't send you information you don't ask for.


I would be inclined to an issue in the interaction between the PIC and Servo rather than some other issue.

Also, make sure you have the COMM at the precise baud rate. (ie check the dividers on your PIC) and that the connections are well soldered and/or shielded to avoid interference and noise.

Finally (this is a bit of a basic one but anyway here it goes) make sure that GND on the servos is the same / shared GND by the PIC. The level of GND is the reference for the data line to identify 0s and 1s.

If the PIC and the servos use different GND lines then the devices may not be seeing the necessary level shifts/differences between GND and DATA line on each end.

Regards
Pedro.