[Axon] Need help in Programming Servos to remain tight...

[Hasan999]

Yes, I have asked a similar question before, Admin gave me the hint... but now I'm confused about one thing*

Here's my code: (It is suppose to set all the servos to their initial positions - and it does)

Code: [Select]

//Initial Servo Positions: (forming a Quadped Robot)
short int x[] = {0,650,690,660,700,700,700,715,710,235,1130,275,1090,330,1105,270,1090,650,650,640,650,670,575};
char lp;
#define d(x)  delay_ms(x)

#define SX(s)  switch (s) { //all the 'cases' are actually in the same line !!

case 1:servo(PORTJ,6,x[1]);break;
case 2:servo(PORTA,6,x[2]);break;
case 3:servo(PORTE,7,x[3]);break;
case 4:servo(PORTH,2,x[4]);break;
case 5:servo(PORTC,1,x[5]);break;
case 6:servo(PORTA,5,x[6]);break;
case 7:servo(PORTE,2,x[7]);break;
case 8:servo(PORTH,6,x[8]);break;
case 9:servo(PORTC,0,x[9]);break;
case 10:servo(PORTA,4,x[10]);break;
case 11:servo(PORTE,3,x[11]);break;
case 12:servo(PORTH,5,x[12]);break;
case 13:servo(PORTC,3,x[13]);break;
case 14:servo(PORTA,2,x[14]);break;
case 15:servo(PORTE,4,x[15]);break;
case 16:servo(PORTH,4,x[16]);break;
case 17:servo(PORTC,4,x[17]);break;
case 18:servo(PORTA,1,x[18]);break;
case 19:servo(PORTE,5,x[19]);break;
case 20:servo(PORTH,3,x[20]);break;
case 21:servo(PORTA,7,x[21]);break;
case 22:servo(PORTA,3,x[22]);break; }

#define SSS for(int i=1;i<24;i++) { SX(i); }
//Note: there are 23 loops, but runs 22 servos !

for(lp=0;lp<10000;lp++) { d(20); SSS; }

The Problem: the servos vibrates in their own positions, and since it has 6 servos in each leg, the first servo's vibration causes enough oscillation at the end positions (the feet)...

*as the Admin said last time, that each for loop will cause 1ms delay by itself?... in that case, does my code exceed 20ms even before it could run the same servo again?... if so, how can I change my code so that all the servos get pulses every after 20ms while keeping the same algorithm !! (because, I have further programming [i excluded here] that is based on this algorithm)

Also, can a servo really get damaged, if I don't keep delays in between? like for example: for( ;; ) {servo(PORTA,1,650);} ?

Thanks

 

[Razor Concepts]

You can reduce the delay and the servos will not get damaged.

Note that your code is incorrect. You should have 22 loops for the 22 servos. It should read i<23; in the for loop.

Additionally, you are controlling each servo individually. You send one signal to one servo, then send one signal to the next servo, then the next, and then after ALL the servos get their signals you delay. So basically, each servo actually gets roughly a 22 times 1.5ms plus 20ms delay (~50ms)

If you wish to control many servos at one time, you cannot use the servo() command. You will have to figure out some code that handles all of the servos simultaneously.

[Hasan999]

I don't understand 1 thing...

How does RoboRealm software keeps all the servos tight without vibration !!?

How can I transform my Algorithm, to that used by RoboRealm ? - I checked servo_controller.c (attached) that RoboRealm uses, and there are also 2 for loops with arrays that control upto 29 servos without causing any vibration at all !!...

Please clarify !! - Thanks

[Razor Concepts]

All the servo pulses are sent at the same time.

For example, say you are controlling 4 servos. 1 servo needs to be sent a 1ms signal, 2 servos need to be sent a 1.5ms signal, and the last servo needs to be sent a 2ms signal - all at the same time.

What do they all have in common? First, all servos need at least a 1ms signal. So all ports are turned high for 1ms. Then, the program sees that the one servo only needs a 1ms signal, so it turns off that port. Then, .5ms later, it sees that two servos need a 1.5ms signal, so those two ports are turned off. Finally, at 2ms, the program sees the last servo needs a 2ms signal, and then that port is turned off. Now that all the servos have received their pulse, delay 20ms and then repeat.

Now, do this same thing, except with as many servos as you need, and with greater resolution in the pulse length.

This way, the signal for ALL servos, no matter how many, can be sent in a single 2ms time period, instead of one after the other.

[Hasan999]

  !!!!!!!!!!

Thanks Razor... so thats the trick... i'll try to change my code that follows RoboRealm algorithm !

btw, is Sorting necessary? - I don't know why they sorted the servo[arrays] ?

I'll try the programming after some hours.. I'd appreciate some more hints in the meanwhile, that could be helpful for such programming..(like what all should I include and exclude just to get all the servos get pulses every after 20ms - because there are a lot of things in servo_controller.c - I'm still a little confused)

Thanks

[Razor Concepts]

I think it is helpful to sort, since the program has to check which servo ends at what pulse time, it would be easier for the program to have a sorted array so that it knows that the next servo in the list will always be the next servo to stop the pulse.

[Hasan999]

  I can't understand how to deal with the first part of servo_controller.c (for RoboRealm)

The whole code:

Code: [Select]


#include "SoR_Utils.h" //includes all the technical stuff

/*************************************************
Protocol:

identification
1st byte - command id = 128

servo_on
1st byte - command id = 129
2nd byte - servo number
3rd byte - CRC

servo_off
1st byte - command id = 130
2nd byte - servo number
3rd byte - CRC

set_servo_position
1st byte - command id = 131
2nd byte - servo number
3rd byte - servo position (bits 1-4)
4th byte - servo position (bits 5-8)
5th byte - CRC for last 4 bytes

All bytes need to be < 128 such that the 5th byte can also be used as a sync byte in case a byte
is lost during transmission.

If two sync bytes are seen Axon responds with the word Axon in order for the PC to know that it is
communicating correctly.

***************************************************************************
*/

// servo position prior to sorting
int init_servo_position[29]={-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1};
// base servo number
int init_servo_number[29]={6,5,4,3,2,7,6,5,4,3,2,1,0,1,2,3,4,5,6,7,6,7,6,5,4,3,2,1,0};
// base servo port
int *init_servo_port[29]={&PORTH,&PORTH,&PORTH,&PORTH,&PORTH,&PORTE,&PORTE,&PORTE,&PORTE,&PORTE,&PORTE,&PORTE,&PORTA,&PORTA,&PORTA,&PORTA,&PORTA,&PORTA,&PORTA,&PORTA,&PORTJ,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC,&PORTC};

// sorted servo position
int servo_position[29];
// sorted servo number
int servo_number[29];
// sorted servo port
unsigned int *servo_port[29];

// cmd variables are used to maintain state for incoming characters
// such that the above protocol can be implemented.
int cmd_index = 0;
int cmd_id;
int cmd_servo_number;
int cmd_servo_position_1;
int cmd_servo_position_2;
int cmd_crc;

// defines the timing period of min and max servo values. You may need to adjust these for your servos
#define MIN_SERVO_TIME 275
#define MAX_SERVO_TIME 1150
float fact = ((MAX_SERVO_TIME-MIN_SERVO_TIME)/255.0);

// is called when a new character from the PC is available
void get_data(unsigned char c)
{
cli();
//rprintf("%c", c);

// toggle LED
PORTB ^= (1<<6);

// check for our sync byte ... which is also the id
if (c&128)
{
cmd_index=0;
cmd_id = (int)c;
}

// id 128 is use for the PC to know that this is an Axon program
if (cmd_id==128)
{
rprintf("Axon\n");
}
else
// enable or disble a servo. When a servo is disabled it can be manually moved.
if ((cmd_id==129)||(cmd_id==130))
{
if (cmd_index==1)
{
cmd_servo_number=c;
}
else
if (cmd_index==2)
{
cmd_crc=c;

if (((cmd_id^(cmd_servo_number<<1))&127)==cmd_crc)
{
if (cmd_id==129)
init_servo_position[cmd_servo_number]=MIN_SERVO_TIME+(((MAX_SERVO_TIME-MIN_SERVO_TIME)/255)*128);
else
init_servo_position[cmd_servo_number]=-1;
}
}
}
else
// used to set a servo position. Note that you need 4 bytes of info for this.
if (cmd_id==131)
{
if (cmd_index==1)
cmd_servo_number=c;
else
if (cmd_index==2)
cmd_servo_position_1=c;
else
if (cmd_index==3)
cmd_servo_position_2=c;
else
if (cmd_index==4)
{
cmd_crc = (int)c;
// CRC is a simple XOR with each successive byte shifted by one more bit
if (((cmd_id^(cmd_servo_number<<1)^(cmd_servo_position_1<<2)^(cmd_servo_position_2<<3))&127)==cmd_crc)
{
// construct the actual value ... we cannot use 1 byte for this since
// a servo value ranges from 0-255 which would cause it to be interpreted
// as a sync byte for values above 128. Thus we need to use two bytes
// to contain half the information.
int pos = cmd_servo_position_1|(cmd_servo_position_2<<4);
init_servo_position[cmd_servo_number]=MIN_SERVO_TIME+(fact*pos);
}
}
}

// increment our data counter
cmd_index++;
}

int main(void)
{
int i,j;

uartInit();  // initialize the UART (serial port)
    uartSetBaudRate(0, 38400); // set UARTE speed, for Bluetooth
    uartSetBaudRate(1, 115200); // set UARTD speed, for USB connection
    uartSetBaudRate(2, 38400); // set UARTH speed
    uartSetBaudRate(3, 38400); // set UARTJ speed, for Blackfin

rprintfInit(uart1SendByte);// initialize rprintf system and configure uart1 (USB) for rprintf

configure_ports();

timer0Init();

uartSetRxHandler(1, &get_data);

//initialize ISR:
UCSR1B |= (1 << RXCIE1); // Enable the USART Recieve Complete interrupt (USART_RXCn)
sei();//turn on the Global Interrupt Enable flag

while (1)
{
// copy all initial values prior to sorting
memcpy(servo_position, init_servo_position, sizeof(servo_position));
memcpy(servo_port, init_servo_port, sizeof(servo_port));
memcpy(servo_number, init_servo_number, sizeof(servo_number));

// sort based on position information and keep track of number and port
// we're using a quick and dirty bubble sort which for 29 values should
// be ok. You could instead use an insertion sort which might be a little
// quicker per loop iteration as we don't expect the position to change
// frequently, but with the (N*N)/2 and flag optimizations this should
// function fine.
int isDone=0;
for (i=0;(i<29-1)&&(!isDone);i++)
{
isDone=1;
for (j=0;j<29-i-1;j++)
{
if (servo_position[j]>servo_position[j+1])
{
// swap servo position
int tmp = servo_position[j];
servo_position[j] = servo_position[j+1];
servo_position[j+1] = tmp;

// keep number and port in sync with the position so we know which port to change!
tmp = servo_number[j];
servo_number[j] = servo_number[j+1];
servo_number[j+1] = tmp;

tmp = servo_port[j];
servo_port[j] = servo_port[j+1];
servo_port[j+1] = tmp;

isDone=0;
}
}
}

//turn off interrupts .. this is very important otherwise your servo will jitter!
cli();

// set all the ports to on that are enabled (i.e.>0)
for (i=0;i<29;i++)
{
if (servo_position[i]>0)
{
PORT_ON(*servo_port[i], servo_number[i]);
}
}

// delay until the last position has occured
int offset=0;
for (i=0;i<29;i++)
{
if (servo_position[i]>0)
{
if (servo_position[i]>offset)
{
// wait until this servo's position offset
delay_cycles(servo_position[i]-offset);
offset=servo_position[i];
}
// turn off this servo and continue with next servos
PORT_OFF(*servo_port[i], servo_number[i]);
}
}

// renable interrupts so that we can receive UART/serial values
sei();

// wait for a total of 20ms from the last servo timing value.
timerPause((4666-offset)/234);
}
}


Whatever is inside main(void) I can understand... but I think the first part (inside) void get_data contains some programming to get the positions of each servo directly from RoboRealm software? right? ...

Question is, how can I remove the RoboRealm software part from the code, so that I can "program" the servos that would work on "interrupts" ?

Thanks..

[madsci1016]

Hasan, it is getting a bit frustrating trying to help you. Not only is this a repeat post, but I answered this question

How does RoboRealm software keeps all the servos tight without vibration !!?

The last time you asked that. Not only are you asking it again, but the source file YOU post has the answer.

The timing of the servos needs to be done in a staggered fashion. You cannot
*    turn on the servo, wait and then turn it off as with 29 servos you will quickly
*   exceed the 20ms base period of most servos. Thus the technique instead will
*   turn on all servos at once and then independantly turn off those servos in
*   succession based on their timing period. For example, if you have two seros
*   at 1ms and 1.5ms phase you would turn both on, wait for 1ms, turn off the first
*   servo, then wait 0.5ms and finally turn off the second. This requires the timing
*    to be sorted in an ascending manner such that the delay from one servo to the
*   next can be easily calculated. While ideally this would be implemented in a
*   parallel manner the short delay due to a single thread is acceptible to most
*   servos.

And this is how they implemented this logic into code:

Code: [Select]

// set all the ports to on that are enabled (i.e.>0)
for (i=0;i<29;i++)
{
if (servo_position[i]>0)
{
PORT_ON(*servo_port[i], servo_number[i]);
}
}

// delay until the last position has occured
int offset=0;
for (i=0;i<29;i++)
{
if (servo_position[i]>0)
{
if (servo_position[i]>offset)
{
// wait until this servo's position offset
delay_cycles(servo_position[i]-offset);
offset=servo_position[i];
}
// turn off this servo and continue with next servos
PORT_OFF(*servo_port[i], servo_number[i]);

There is a problem with this however, like i explained in your last thread. If you need to do a lot of processing at the same time, you will slow down the servo loops until you get jitter again. That is why i recommended getting an external servo controller in addition to your microcontroller.