Servos twitching at one end

Go To Last Post
14 posts / 0 new
Author
Message
#1
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi,

I am working on a project to make a robot arm. I have started testing by driving 2 servos one for the shoulder movement and the other for the elbow. I am controlling the servos using a PS2 controller. The servos are driven by OCR1A and OCR1B. The problem is that if both the servos are connected to pins PD4 and PD5 of ATmega16, they start twitching and don't move correctly. However, if I disconnect any one servo, the other one moves alright, but on reaching one end it starts twitching again.

Here is the code:

#define SERVO_PORT  PORTD
#define SERVO_PORT_DDR  DDRD

void servo_Init() 
{
	SERVO_PORT_DDR |= _BV(PD4) | _BV(PD5);

	TCCR1A |= _BV(COM1A1) | _BV(COM1B1) | _BV(WGM11);	//Non-inverted PWM
	TCCR1B |= _BV(WGM13) | _BV(WGM12) | _BV(CS11) | _BV(CS10);	//Prescaler = 64, Mode = 1

	ICR1 = 4999;	//fPWM=50Hz (Period = 20ms Standard).for 16 MHz clock and Prescalar 64
}

Here is the code for controlling the servo in the program's main while loop.


/*******************************************
		Servo motor control
		*******************************/
		
		//Turn servo clockwise if right joystick is pressed right
		if(ps2.rx == 0x00) {	//if right Joystick pressed right
			if(servo_index == 0) {
				servo_index = rotate_shoulder_servo(0);
			} else {
				servo_index = rotate_shoulder_servo(servo_index);
			}
		}

		//Turn servo anti-clockwise if right joystick is pressed left
		if(ps2.rx == 0xFF) {	//if right Joystick pressed left
			if(servo_index == 16) {
				servo_index = rotate_shoulder_servo(16);
			} else {
				servo_index = rotate_shoulder_servo(servo_index);
			}
		}

		//Turn servo clockwise if right joystick is pressed up
		if(ps2.ry == 0x00) {	//if right Joystick pressed up
			if(servo_index == 0) {
				servo_index = rotate_elbow_servo(0);
			} else {
				servo_index = rotate_elbow_servo(servo_index);
			}
		}

		//Turn servo anti-clockwise if right joystick is pressed down
		if(ps2.ry == 0xFF) {	//if right Joystick pressed up
			if(servo_index == 16) {
				servo_index = rotate_elbow_servo(16);
			} else {
				servo_index = rotate_elbow_servo(servo_index);
			}
		}

The two functions are defined below:

uint8_t rotate_shoulder_servo(uint8_t servoIndex) {
	uint8_t i;
	ps2_poll(ps2.pressure[5],PS2_MOTOR_OFF);
	if(ps2.rx == 0x00) {
		for(i = servoIndex; i < 17; i++) {
			ps2_poll(ps2.pressure[5],PS2_MOTOR_OFF);
			if(ps2.rx != 0x00) {
				break;
			}
			OCR1A = servo_positions[i];
			delay_servo();		
		}
	} else {
		for(i = servoIndex; i > 0; i--) {
			ps2_poll(ps2.pressure[5],PS2_MOTOR_OFF);
			if(ps2.rx != 0xFF) {
				break;
			}
			OCR1A = servo_positions[i];
			delay_servo();
		}
	}
	return i;
}


uint8_t rotate_elbow_servo(uint8_t servoIndex) {
	uint8_t i;
	ps2_poll(ps2.pressure[5],PS2_MOTOR_OFF);
	if(ps2.ry == 0x00) {
		for(i = servoIndex; i < 17; i++) {
			ps2_poll(ps2.pressure[5],PS2_MOTOR_OFF);
			if(ps2.ry != 0x00) {
				break;
			}
			OCR1B = servo_positions[i];
			delay_servo();		
		}
	} else {
		for(i = servoIndex; i > 0; i--) {
			ps2_poll(ps2.pressure[5],PS2_MOTOR_OFF);
			if(ps2.ry != 0xFF) {
				break;
			}
			OCR1B = servo_positions[i];
			delay_servo();
		}
	}
	return i;
}

Do you see anything wrong with the code? Also even when i'm driving a single servo, why does it start twitching when it reaches one end?

Thanks,
Sumair

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I forgot to mention the different servo positions which i have placed in an array.

const uint16_t servo_positions[17] = {150, 178, 206, 235, 263, 291, 319, 347, 375, 404, 431, 460, 488, 516, 544, 572, 600};

I just run through the array in the forward direction to move it clockwise and then move in the reverse direction to move it anti-clockwise.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Your code has Parkinson's. Seriously, have you looked at your output with an oscilloscope to determine what is happening? What? You have no oscilloscope? Maybe you do if you have a soundcard in your PC. Google for 'soundcard oscilloscope'

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

If this is standard servos that's controlled by a 1-2 ms wide pulse, repeated about 50 times per second, then if this is correct

ICR1 = 4999;   //fPWM=50Hz (Period = 20ms Standard).for 16 MHz clock and Prescalar 64 

you have 5000/20 = 250 ticks per millisecond. So 250 is 1 ms and 500 is 2ms, but you vary the signal between 150 (0.6 ms) and 600 (2.4 ms).

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

In one of the forum posts i had seen that you can verify the servo signal between 0.6 ms and 2.4 ms which is why i had specified these values. I guess i need to check my servo datasheet first.
The second problem i noticed in my code is that the value of ICR1 maybe wrong because of a wrong formula posted in one of the forums.

I'll recheck these 2 things first.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Sounds like an electronic/PSU issue to me. Are the servos powered from the same source as the AVR - what happens on Vcc (monitored with a scope) when both servos are connected and being commanded?

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

ssyed_mcu wrote:
In one of the forum posts i had seen that you can verify the servo signal between 0.6 ms and 2.4 ms.
I haven't done much work with servos, but once I did test the limits on two servos (same model and brand). One of them could handle pulses between 0.84 and 2.4 ms. For the other one the limits were 0.74 to 2.3 ms. So I don't think you can take 0.6-2.4 as a rule.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Quote:
So I don't think you can take 0.6-2.4 as a rule.
And especially not for the first test of a servo.
Better start with 1.1 - 1.9 ms. Then find both limits experimentally.

PS
Also, I would start with a simplier code, e.g. only set servo to the center.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Hi,

Thanks for all your replies. I found out what the problem was and have fixed it. The formula used to calculate the value of ICR1 was wrong. After using the correct formula the correct ICR1 value is 20000.

void servo_Init() 
{
	SERVO_PORT_DDR |= _BV(PD4) | _BV(PD5);

	TCCR1A |= _BV(COM1A1) | _BV(COM1B1);	//Non-inverted PWM
	TCCR1B |= _BV(WGM13) | _BV(CS11);	//Prescaler = 8, phase and frequency correct mode

	ICR1 = 20000;	//fPWM=50Hz (Period = 20ms Standard).for 16 MHz clock and Prescalar 8
}

Also instead of defining an array with pre-determined servo positions, I am just running through all possible OCR1A values as shown below:

ps2_poll(ps2.pressure[5],PS2_MOTOR_OFF);

		if(ps2.rx == 0xFF) {
			if((servo1 == MIN_SERVO) && (servo1 <= MAX_SERVO)) {
				rotate_servo1_forward_from_start();
			} else if((servo1 > MIN_SERVO) && (servo1 <= MAX_SERVO)) {
				rotate_servo1_forward_from_middle();
			}
		}

The functions for rotating the servo are as below:

void rotate_servo1_forward_from_start() {
	ps2_poll(ps2.pressure[5],PS2_MOTOR_OFF);
	for(OCR1A = MIN_SERVO; OCR1A < MAX_SERVO; OCR1A++) {
		ps2_poll(ps2.pressure[5],PS2_MOTOR_OFF);
		if(ps2.rx != 0xFF) {
			servo1 = OCR1A;
			break;
		}
		_delay_ms(10);
	}
}

void rotate_servo1_forward_from_middle() {
	ps2_poll(ps2.pressure[5],PS2_MOTOR_OFF);
	for(OCR1A = servo1; OCR1A < MAX_SERVO; OCR1A++) {
		ps2_poll(ps2.pressure[5],PS2_MOTOR_OFF);
		if(ps2.rx != 0xFF) {
			servo1 = OCR1A;
			break;
		}
		_delay_ms(10);
	}
}

I have similar functions for rotating the servo in the opposite directions. With this I'm able to move the servo quite smoothly.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

I determined the values of MIN_SERVO as 500 and MAX_SERVO as 2500 with a little experimentation.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Quote:

After using the correct formula the correct ICR1 value is 20000.

Something doesn't add up here.

Quote:

TCCR1B |= _BV(WGM13) | _BV(CS11); //Prescaler = 8, phase and frequency correct mode

ICR1 = 20000; //fPWM=50Hz (Period = 20ms Standard).for 16 MHz clock and Prescalar 8


If 20000 counts indeed represents 20ms, that would be 1000 timer counts per millisecond, or one timer count is one microsecond.

If your AVR clock is indeed 16MHz and that setting is indeed for /8, then every timer count would be two microseconds.

So, your period could be 25Hz--which may not be a problem. Or your AVR might be running at 8MHz. Again, that may not be a problem. But if your AVR is running off the internal oscillator at 8MHz you will need some calibration to do accurate work.

You can put lipstick on a pig, but it is still a pig.

I've never met a pig I didn't like, as long as you have some salt and pepper.

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Quote:
The formula used to calculate the value of ICR1 was wrong.
No, the ICR value was right in your original code.

There you used
CTC mode 14
F_CPU = 16MHz
prescaler = 64
ICR1 = 4999

The formula for CTC mode is
ICR1 = period[ms] * F_CPU[Hz] / (1000 * prescaler) - 1

ICR1 = 20 * 16000000 / (1000 * 64) - 1 = 4999

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

ssyed_mcu wrote:

			if(servo_index == 0) {
				servo_index = rotate_shoulder_servo(0);
			} else {
				servo_index = rotate_shoulder_servo(servo_index);
			}


Isn't that the same as :

servo_index = rotate_shoulder_servo(servo_index);

The same goes for your other calls to rotate_XXX_servo().

Sid

Life... is a state of mind

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

It looks to me like rotate_XXX_servo() may return 17. If that value is later passed back in to rotate_XXX_servo(), you may end up accessing servo_positions[17] - which is undefined.

Also, the loops accessing servo_positions[] in reverse order never uses servo_positions[0].

Sid

Life... is a state of mind