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
