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