New: alternative firmware for AX12

[limor]

Ricardo has been playing around with AX12 firmware.
(see http://actuated.wordpress.com)
The process of replacement is quite simple:
1) AVRStudio compiles the code, exports hex.
2) hex2bin converts to bin file.
3) using USB2Dynamixel (+ SMPS2Dynamixel to power the servo), connect servo and in robot-terminal connect to the line at 56700 baud.
4) press '#' continuously when powering the servo; bootloader prompt appears
5) use the command "load" to load the firmware just like with CM5

The following code is a preliminary "sketch". After loading it, keep the robot-terminal open. It reads the characters from the serial port and expects either + or - and increases/decreases the speed of the motor, printing out the value from the potentiometer.

Code: Select all

#include "avr/delay.h"
#include "avr/io.h"
#include "avr/iom8.h"
#include "avr/interrupt.h"
#include "ctype.h"

#define BIT0 0x1
#define BIT1 0x2
#define BIT2 0x4
#define BIT3 0x8
#define BIT4 0x10
#define BIT5 0x20
#define BIT6 0x40
#define BIT7 0x80


#define AX12_PING      0x01
#define AX12_READ      0x02
#define AX12_WRITE      0x03
#define AX12_REG      0x04
#define AX12_ACTION      0x05
#define AX12_RESET      0x06
#define AX12_SYNC_WR   0x83


#define HalfDuplexTransceiver_Port      PORTD
#define HalfDuplexTransceiver_Dir      DDRD
#define HalfDuplexTransceiver_PinOUT   BIT7
#define HalfDuplexTransceiver_PinIN      BIT6

#define Baud9600    207
#define Baud57600   34
#define Baud115200  16
#define Baud500000  3
#define Baud1000000 1
#define Baud2000000 0



#define bit(param) (1<<param>0)?1:0)
#define TXBuffEmpty (((UCSRA&bit(TXC))>0)?1:0)


#define SET_WRITE HalfDuplexTransceiver_Port|=HalfDuplexTransceiver_PinOUT;HalfDuplexTransceiver_Port&=~HalfDuplexTransceiver_PinIN;

#define SET_READ HalfDuplexTransceiver_Port&=~HalfDuplexTransceiver_PinOUT;HalfDuplexTransceiver_Port|=HalfDuplexTransceiver_PinIN;


#define Position_Dir       DDRC
#define Position_Port      PORTC
#define Position_Pin      BIT0
#define Temperature_Dir       DDRC
#define Temperature_Port   PORTC
#define Temperature_Pin      BIT2




#define SERVOBuffer_MAX_SIZE 100

typedef struct SERVO_DATA_STRUCT
{
   uint8_t u8ServoID;
   uint8_t u8ServoInstruction;
   uint8_t u8ServoDataLength;
   uint8_t u8ServoData[SERVOBuffer_MAX_SIZE];
} stServo_Data;



#define UART_Baudrate Baud57600


#define Led      BIT2
#define Led_Port PORTD
#define Led_Dir  DDRD



#define LED_ON  Led_Port &= ~Led;
#define LED_OFF Led_Port |= Led;


#define Motor_Clockwise    BIT1
#define Motor_Anticlockwise   BIT2
#define Motor_Port          PORTB
#define Motor_Dir          DDRB

//#define RotateClockwise     Motor_Port |= Motor_Clockwise;Motor_Port &= ~(Motor_Anticlockwise);
//#define RotateAnticlockwise Motor_Port |= Motor_Anticlockwise;Motor_Port &= ~(Motor_Clockwise);
//#define RotateStop         Motor_Port &= ~(Motor_Clockwise|Motor_Anticlockwise);


uint16_t PWM=1000;
uint16_t Temperature=0;
uint16_t Position=0;




void StopMotor(void)
{
   TCCR1A &= ~(bit(COM1A1)|bit(COM1B1));
   OCR1A = OCR1B = 0;
   Motor_Port&= ~(Motor_Clockwise|Motor_Anticlockwise);
}



int8_t SetPWM(uint16_t pwm)
{
   
   TCCR1A &= ~(bit(COM1A1)|bit(COM1B1));
   OCR1A = OCR1B = 0;
   Motor_Port&= ~(Motor_Clockwise|Motor_Anticlockwise);
   if(pwm==1000)
   {
      TCCR1A &= ~(bit(COM1A1)|bit(COM1B1));
      OCR1A = OCR1B = 0;
      Motor_Port&= ~(Motor_Clockwise|Motor_Anticlockwise);
      return 0;
   }
   else
   if((pwm>1000) && (pwm<=2000))
   {
      TCCR1A &= ~(bit(COM1A1)|bit(COM1B1));
      Motor_Port&= ~(Motor_Anticlockwise);
      TCCR1A |= bit(COM1A1);
      OCR1A = (pwm-1000);
      return 1;
   }
   else
   if((pwm<1000>=0))
   {
      TCCR1A &= ~(bit(COM1A1)|bit(COM1B1));
      Motor_Port&= ~(Motor_Clockwise);
      TCCR1A |= bit(COM1B1);
      OCR1B = (1000-pwm);
      return 1;
   }
   return -1;
}


void InitTimer(void)
{
   //InitTimer0
   TCCR0|=bit(CS01);//assuming the clkIO is 16MHZ, the prescaler devided by 64 wich mean that for 100 positions
   //TCCR0|=bit(CS01);                     // PWM we have 250KHz of timer, meaning this 2500Hz of PWM
   TIMSK |= bit(TOIE0);//enable interrupt
   TCNT0=0x00;


   //Init Timer 1 for PWM
   Motor_Port&= ~(Motor_Clockwise|Motor_Anticlockwise);   

   TCNT1 = 0;
   TCCR1A = 0;
   TCCR1B = 0;
   //TCCR1C = 0;
   //TIMSK1 = 0;

   ICR1=1000; // 10KHz PWM
   
   TCCR1B = (bit(WGM13)|bit(CS10));
   // Set the PWM duty cycle to zero.
   
   OCR1A = 0;
   OCR1B = 0;
   
   StopMotor();
   
}





uint16_t POSITION_AD(void)
{
   uint16_t returned=0;
   uint8_t temp=0;
   while(ADCSRA&bit(ADSC));
   //ADCSRA&=bit(ADIF);
   ADMUX&=~(bit(MUX0)|bit(MUX1)|bit(MUX2)|bit(MUX3));// this select the ad multiplexer to channel ADC0 position input
   //ADCSRA|=bit(ADEN);  // start the conversion processes
   ADCSRA|=(bit(ADSC)|bit(ADEN));  // start the conversion processes
   while(ADCSRA&bit(ADSC));//wait for the end
   temp = ADCL;
   returned = ADCH;
   returned = (returned<<8);
   returned += temp;
   ADCSRA&=(bit(ADEN));
   return ( ADCL + (ADCH<<8)); // this represents a 16bit variable
}


uint16_t TEMPERATURE_AD(void)
{
   uint16_t returned=0;
   uint8_t temp=0;
   while(ADCSRA&bit(ADSC));
   //ADCSRA&=bit(ADIF);
   ADMUX&=~(bit(MUX0)|bit(MUX2)|bit(MUX3));// this select the ad multiplexer to channel ADC0 position input
   ADMUX|=bit(MUX1);
   //ADCSRA|=bit(ADEN);  // start the conversion processes
   ADCSRA|=(bit(ADSC)|bit(ADEN));  // start the conversion processes
   while(ADCSRA&bit(ADSC));//wait for the end
   temp = ADCL;
   returned = ADCH;
   returned = (returned<<8);
   returned += temp;
   ADCSRA&=(bit(ADEN));
   return ( ADCL + (ADCH<<8>0)
   {
      UDR=*Array++;
      while(!TXByteEmpty);
      counter--;
   }
   while(!TXBuffEmpty);
   UCSRA|=bit(TXC);
   SET_READ;

}


void InitSerial(uint16_t SerialSpeed)
{
   
   

   
   UCSRA = bit(U2X);    
   //using double speed
   UCSRB = (/*bit(RXCIE)|bit(UDRIE)|*/bit(RXEN)|bit(TXEN)/*|bit(TXCIE)*/); // enable RX and TX interrupt RX and TX
   UCSRB &= ~(bit(RXB8)|bit(TXB8)|bit(UCSZ2));   // disable  TX and RX nineth bit and reset UCSZ2, this is used with UCSZ1 and UCSZ0 to determine the length of the word
   UCSRC = ~(bit(UMSEL)|bit(UPM0)|bit(UPM1)|bit(USBS)|bit(UCPOL));//disable parity and set assyncronous, set 1stopBit and polarity on rising edge
   UCSRC |= (bit(UCSZ1)|bit(UCSZ0)|bit(UCPOL));
   //   UCSZ2=0 UCSZ1=1 UCSZ0=1 => 8bit length
   UBRRH = ((SerialSpeed)>>8);   //set baud counter high
   UBRRL = (SerialSpeed&0xFF);          //set baud counter low                                                                                             
   //sei();
   UCSRA|=bit(TXC);

}


void PrintDecimal (int Data)
{

   volatile uint16_t counter=5, firstNum=0, result=0, temp=0;
   volatile uint16_t divisor=10000;

   if(Data>0)
   {
      while(counter--)
      {
         result=Data/divisor;
         if( (result>0) || (firstNum==1))
         {
            firstNum=1;
            result+=0x30;
            SendArray((uint8_t *)&result,1);
            result-=0x30;
         }
         Data-=(result*divisor);
         divisor=(divisor/10);
      }
   }
   else
   if(Data<0>0) || (firstNum>0))
         {
            firstNum++;
            result+=0x30;
            if(firstNum==1)
            {
               SendArray("-",1);
            }

            SendArray((uint8_t *)&result,1);
            result-=0x30;
         }
         Data-=(result*divisor);
         divisor=(divisor/10);
      }
   }
   else
   {
      result=0x30;
      SendArray((uint8_t*)&result,1);
   }

}




uint8_t GetChar(uint8_t *c)
{
   if(UCSRA&bit(RXC))
   {
      *c=UDR;   
      return 1;
   }

   return 0;
   
}

void main (void)
{

   uint8_t c=0;
   WDTCR=0;

   Motor_Dir|= (Motor_Clockwise|Motor_Anticlockwise);
   HalfDuplexTransceiver_Dir  |=   (HalfDuplexTransceiver_PinOUT|HalfDuplexTransceiver_PinIN);
   Led_Dir |= Led;
   InitSerial(UART_Baudrate);
   InitTimer();
   //InitADs();
   //RotateStop
   LED_OFF
   sei();
   PWM=1000;
   //ADCSRA&=bit(ADIF); // clear adc interrupt flag
   //SendArray("\n",1);
   while(1)
   {
      //SendArray("Ola Azeiteirinhu\n\r",18);

      if(GetChar(&c))
      {
         if(c==0x2B)
         {
            if(PWM<1990>=10)
            {
               PWM-=10;
               SetPWM(PWM);
            }
            SendArray("\n\rPWM: ",7);
            PrintDecimal(PWM);
         }

         
      }

      //SET_READ;
      /*   SendArray("\rPosition: ",12);
      PrintDecimal(Position);
      SendArray("   Temperature: ",16);
      PrintDecimal(Temperature);
      SendArray("     ",5);*/
      
   }
}




[billyzelsnack]

Sweet. I was under the impression that something goofy was going on that prevented this. Guess not. This opens up a lot of possibilities.

[limor]

We are working on is getting the Openservo's PID code running inside AX12

ultimately the plan is to develop new firmware with these specs:

1) proper speed control. Current Dynamixel firmware lacks real speed control. it is only good at holding position. Speed control is acheived by changing the amount of current going to the motor to maintain a constant speed.

2) a proper control loop between the bus-master and the servos. Currently with Dynamixel the most efficient method is using [billyzelsnack's coined term] SYNC_READ. ie: bus-master asks the servos their position one after the other and then tells them where to go using one long packet SYNC_WRITE. With 12 servos we reached about 170 cycles/sec (where the CM5 is bus-master for a bunch of AX12 servos on the bus

= We expect to get 2 times speed improvement : CM5 - AX12
= We expect to get about at least 150 cycles/sec : Linux - USB2Dynamixel - AX12

[limor]

New firmware has been working for a couple of days. Source code will soon be available. The following video shows the AX12 running the new firmware and maintaining constant speed unlike any AX12 with standard dynamixel firmware.

phpBB [media]

[MOHIT JINDAL]

Do you have a code program to control 18 Servos from Usb2Dynamixel ?

Mail me at mohitjindal_niit@yahoo.co.in

[billyzelsnack]

MOHITJINDAL.
http://support.robotis.com/en/software/dynamixelsdk.htm

[limor]

So we have speed control of the AX12.

This draft of the AX12 firmware provides 2 control states.

Speed control - modifies the PWM duty cycle to compensate for varying for external torque and maintain fixed speed

PD position control - once target position is reached, a kind of PID control is applied in order to create a "spring effect" around the target position.

You can download and flash the AX12.BIN file and play with it.

- use robot terminal, putty or any other terminal emulator connect at 57600 baud
- connect to a servo through usb2dynamixel + SMP2Dynamixel
(CM5 can be used to power the bus, just make sure it isnt playing any motions and disturbing disturbing the data line)
- when you power the bus, press and hold the '#' key and you will see a bootloader '*' prompt where you can enter a few commands like "help" and "load"

to modify the code:
download AVRSTudio with AVRGCC (free download)
-- compile the project (target CPU is Atmega8, 16mhz)

[limor]

I spent some time looking through the OpenServo source code thinking that these guys at openservo know their stuff when it comes to servo control so why not use their code. and they sure do.

The main loop of openservo looks like this:

Code: Select all

main() {
  ...
   for() {
      if (adc_position_value_is_ready()) { ... do stuff ... }
      if (adc_power_value_is_ready()) { ... }
      if (twi_data_in_receive_buffer()) { .... }
   }
}

registers.c - a set of global variables used everywhere. Instead of just using a bunch of global variables, the registers.c functions provide two functions: registers_read_word(), registers_write_word() - they disable interrupts, then they read/write from the global variables, then re-enable interrupts. this ensures there is no race condition where an interrupt changes values as they are being read or written to.
There are also helper routines like fixed_multiply() which i think are used to multiplies floats or longs.

There are 3 interrupts in use:
- ADC sampling is done every 10ms (timer0). this is also the timeframe for changing any control parameters. The advantage of doing it only 100 times/sec is that you get 6-10 point change in the potentiometer sample. so you can use this to determine speed with some degree of precision (if you sample 1000 times/sec you will see 1-2 point change and this makes it hard to determine speed without a speed estimator or a brutal average).
- TWI - for communications over i2c. In our AX12 this is irrelevant since we use the UART interface at 1mhz.
- pulse - pin0changed interrupt + uses timer2 interrupt to measure pulse length

The openservo can be compiled to different requirements by setting /unsetting #defines.
- React to RC pwm signal instead (in addition to) I2C. The pulse pwm signal may be coming from RC remote control for example.
- Apply IPD control for position control
- Apply PID control for position control (difference?)
- Estimate velocity by emulating the servo mechanical/electrical model (uses floating point calculations)
- Motion control - Control speed of servo and get it to follow a smooth pre-defined curve (Beizer curve, there's a utility you can download from openservo.org to help create these curves with up to 8 waypoints). go through predefined way-points at given time. so you can control a complex real-time path. this is similar to humanoid/hexapod gait control paradigm only the path is stored in the servo. personally i'd prefer to do this from an embedded linux controlling the speed and curves and modifying them to react to the environment.

So if you want IPD control, you just need to set the #define accordingly. the main loop will call the routine and the routine will change the PWM duty cycle that goes to the motor, every time it is called. this happens 100 times/sec every time the ADC is interrupt happens.

About "pulse control" - it waits for RC pwm signal (1ms - 2ms) representing 0..1023 available target positions and goes there at full speed.

Code: Select all

     registers_write_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO, pulse_position);
     registers_write_word(REG_SEEK_VELOCITY_HI, REG_SEEK_VELOCITY_LO, 0);

[billyzelsnack]

Here's an idea. Make a new firmware that makes the mostly useless AX-S1 (also?) do all the busy work for SYNC_READ support.

[i-Bot]

If you were to use the AX-S1 to interpret the sync read, you would probably need to do it different to current implementations, and maybe change the servo firmware too.
Existing sync reads (Arbotix, urbi,..) are point to point implementations which include the dynamixel protocol. They are not used directly on the dynamixel bus with multiple slaves, because they violate the dynamixel protocol. Since all devices on the dynamixel bus track the bus state, this violation may cause confusion due to the new states introduced by bus cycles within bus cycles, and responses to broadcasts.

I guess now we have to await the publication of the dynamixel 2.0 protocol to see what that has in terms of faster bus operation.

[siempre.aprendiendo]

...
I guess now we have to await the publication of the dynamixel 2.0 protocol to see what that has in terms of faster bus operation.

Dynamixel 2.0? Where do you hear/read about it?

[billyzelsnack]

If you were to use the AX-S1 to interpret the sync read, you would probably need to do it different to current implementations, and maybe change the servo firmware too.
Existing sync reads (Arbotix, urbi,..) are point to point implementations which include the dynamixel protocol. They are not used directly on the dynamixel bus with multiple slaves, because they violate the dynamixel protocol. Since all devices on the dynamixel bus track the bus state, this violation may cause confusion due to the new states introduced by bus cycles within bus cycles, and responses to broadcasts.

I guess now we have to await the publication of the dynamixel 2.0 protocol to see what that has in terms of faster bus operation.

I'm not seeing it. The other servos would only respond to their own requests. They can care less whether bus chatter originates from another dynamixel or some USB interface or whatever.

[i-Bot]

The servos listen and track the bus protocol for all transactions. They action only packets addressed to them or the broadcast address. They respond to only their address.
Existing implementations of syncread use the broadcast address and this violates the dynamixel bus protocol.

ID 0XFE is the Broadcasting ID which indicates all of the connected Dynamixel units. Packets sent with this ID apply to all Dynamixel units on the network. Thus packets sent with a broadcasting ID will not return any status packets.

You would need to check that a syncread response did not confuse devices due to the broadcast ID, but with the top bit cleared in the instruction byte. Or use a different address instead of broadcast for the sync read.
Accurate tracking of bus state is important to dynamixel devices, because the 0xFF 0xFF sequence can occur in data.

Also you now have two bus masters. While the usual bus master is waiting for a reply it will see all the AX-S1 messages. I assume your usual bus master is dumb (USB2Dynamixel), else why do this anyway.

[billyzelsnack]

Ok. I think I see what you are talking about now. In my implementation ( http://robosavvy.com/forum/viewtopic.ph ... f330fd7488 ) I had a dynamixel device and the USB2DYNAMIXEL (or whatever) combined. This allowed me to write out to the second UART (over USB) whenever I wanted.

Since the USB2DYNAMIXEL (or whatever) has no knowledge of what data is what, it just passes ALL dynamixel side traffic back over USB. Is this correct?

I'm thinking that even with dynamixel protocol 2.0 this will not be possible without USB2DYNAMIXEL firmware updates. At some point the dynamixel to PC bridge (whatever it is) needs to make a decision on what NOT to send over.