Quadrapod Servo Problem

Support A-Star and Orangutan
1

Hello,

I’m building a quadrapod using 8 servos (the GWS S03T STD) and an Orangutan SVP-324. Right now, I’m working on the program that will control the forward walking motion, but there is a problem.

The following program I’m using is for switching between the different servos, and then determining their ranges and which directions I need the servos to rotate, by using the middle and bottom buttons to increase/decrease the servo rotation. The top button changes servos.

int main()
{
	
	const int NUM_SERVOS = 8;
	
	const unsigned char DEMUX_PINS[] = {IO_B3, IO_B4, IO_C0};	// 8 servos
	
	const int MIN_SERVO_POSITION = 750;
	const int NEUTRAL_SERVO_POSITION = 1500;
	const int MAX_SERVO_POSITION = 2250;
	
	int servo = 0;						// current servo
	int servoPositions[NUM_SERVOS];			// current positions of all servos
	int i;
	
	// wait for top button before doing anything
	clear();
	print("Press TOP btn");
	lcd_goto_xy(0, 1);
	print_long(read_battery_millivolts_svp());
	print("mV");
	wait_for_button_press(TOP_BUTTON);
	wait_for_button_release(TOP_BUTTON);

	// initialize the demux for 8 servos
	servos_start(DEMUX_PINS, sizeof(DEMUX_PINS));
	
	// put the servos into a neutral position
	for(i = 0; i < NUM_SERVOS; i ++)
	{
		set_servo_speed(i, 150);
		set_servo_target(i, NEUTRAL_SERVO_POSITION);
		servoPositions[i] = NEUTRAL_SERVO_POSITION;
	}

	// main loop
	while(1)
	{

		// print information about the current servo
		
		lcd_goto_xy(0, 0);
		print("Servo ");
		print_long(servo);
		print(": ");
		print_long(servoPositions[servo]);
		print("   ");					// put in a blank so we don't have to clear the screen
		lcd_goto_xy(0, 1);
		print("Top: next servo ");
		
		// top button changes servo
		if(button_is_pressed(TOP_BUTTON))
		{
			servo ++;
			if(servo >= NUM_SERVOS)
			{
				servo = 0;
			}
			wait_for_button_release(TOP_BUTTON);
		}
		
		// middle button increases servo position
		if(button_is_pressed(MIDDLE_BUTTON))
		{
			servoPositions[servo] ++;
			if(servoPositions[servo] > MAX_SERVO_POSITION)
			{
				servoPositions[servo] = MAX_SERVO_POSITION;
			}
			set_servo_target(servo, servoPositions[servo]);
		}
		
		// bottom button decreases servo position
		if(button_is_pressed(BOTTOM_BUTTON))
		{
			servoPositions[servo] --;
			if(servoPositions[servo] < MIN_SERVO_POSITION)
			{
				servoPositions[servo] = MIN_SERVO_POSITION;
			}
			set_servo_target(servo, servoPositions[servo]);
		}
		
	}
}

I’ve connected the IO lines B3, B4, and C0 to SA, SB, and SC, respectively. The servos are powered from VADJ, adjusted to 6v.

The program works as expected, with one exception - the servos, being at any position other than neutral, begin jittering quite severely, as if servo target positions are occasionally being sent to the wrong servo. The eight-servo demo program works perfectly fine and does not jitter.

I’ve looked into this as best I can, but unfortunately, cannot find a cause for this problem. Would someone be able to shed some light on this? Thanks.

Geoff

2

Hello, Geoff.

I don’t see anything obviously wrong with your program. My suggestion for tackling this would be to start with the working example program and incrementally modify it to change it to your program. At some point, the servos should start jittering, and that will tell you what part of your program is causing the jitter. If you can post the simplest program with jitter and the simplest program without, maybe I can help you identify what is going on.

- Ben

3

Hi Ben,

Well, this is strange. Apparently, any of lcd_goto_xy(), print(), or print_long() will cause the servos to jitter. My guess is that any other LCD-manipulating functions will have the same effect. According to the Orangutan user’s guide, none of the LCD lines are shared with the servo or demux lines I’m using, though.

Here is the non-working program (modified slightly from the old one):

int main()
{

    const unsigned char DEMUX_PINS[] = {IO_B3, IO_B4, IO_C0}; // 8 servos

	const int NUM_SERVOS = 8;

	const int MIN_SERVO_POSITION = 750;
	const int NEUTRAL_SERVO_POSITION = 1500;
	const int MAX_SERVO_POSITION = 2250;
	
	int servo = 0;							// current servo
	int servoPositions[NUM_SERVOS];			// current positions of all servos
	int i;

	// initialize the demux for 8 servos
	servos_start(DEMUX_PINS, sizeof(DEMUX_PINS));
	
	// print welcome
	
	clear();
	print("Press TOP btn");
	lcd_goto_xy(0, 1);
	print_long(read_battery_millivolts_svp());
	print("mV");
	wait_for_button_press(TOP_BUTTON);
	wait_for_button_release(TOP_BUTTON);
	
	// slowly put the servos into a neutral position
	for(i = 0; i < NUM_SERVOS; i ++)
	{
		set_servo_speed(i, 150);
		set_servo_target(i, NEUTRAL_SERVO_POSITION);
		servoPositions[i] = NEUTRAL_SERVO_POSITION;
	}
 
    while(1)
    {
		
		// any of these lines in this code block seem to interfere with the servos
		lcd_goto_xy(0, 0);
		print("Servo ");
		print_long(servo);
		print(": ");
		print_long(servoPositions[servo]);
		print("   ");							

// put in a blank so we don't have to clear the screen
		lcd_goto_xy(0, 1);
		print("Top: next servo ");

		// top button changes servo
		if(button_is_pressed(TOP_BUTTON))
		{
			servo ++;
			if(servo >= NUM_SERVOS)
			{
				servo = 0;
			}
			wait_for_button_release(TOP_BUTTON);
		}
		
		// middle button increases servo position
		if(button_is_pressed(MIDDLE_BUTTON))
		{
			wait_for_button_release(MIDDLE_BUTTON);
			servoPositions[servo] += 100;
			if(servoPositions[servo] > MAX_SERVO_POSITION)
			{
				servoPositions[servo] = MAX_SERVO_POSITION;
			}
			set_servo_target(servo, servoPositions[servo]);
		}
		
		// bottom button decreases servo position
		if(button_is_pressed(BOTTOM_BUTTON))
		{
			wait_for_button_release(BOTTOM_BUTTON);
			servoPositions[servo] -= 100;
			if(servoPositions[servo] < MIN_SERVO_POSITION)
			{
				servoPositions[servo] = MIN_SERVO_POSITION;
			}
			set_servo_target(servo, servoPositions[servo]);
		}

    }
}

The working one performs the same task, but rather than continuously updating the LCD, only does so after setting the current servo’s target position.

void printServoStatus(int servo, int value)
{
	lcd_goto_xy(0, 0);
	print("Servo ");
	print_long(servo);
	print(": ");
	print_long(value);
	print("   ");							// put in a blank so we don't have to clear the screen
	lcd_goto_xy(0, 1);
	print("Top: next servo ");
}

int main()
{

    const unsigned char DEMUX_PINS[] = {IO_B3, IO_B4, IO_C0}; // 8 servos

	const int NUM_SERVOS = 8;

	const int MIN_SERVO_POSITION = 750;
	const int NEUTRAL_SERVO_POSITION = 1500;
	const int MAX_SERVO_POSITION = 2250;
	
	int servo = 0;							// current servo
	int servoPositions[NUM_SERVOS];			// current positions of all servos
	int i;

	// initialize the demux for 8 servos
	servos_start(DEMUX_PINS, sizeof(DEMUX_PINS));
	
	// print welcome
	
	clear();
	print("Press TOP btn");
	lcd_goto_xy(0, 1);
	print_long(read_battery_millivolts_svp());
	print("mV");
	wait_for_button_press(TOP_BUTTON);
	wait_for_button_release(TOP_BUTTON);
	
	// slowly put the servos into a neutral position
	for(i = 0; i < NUM_SERVOS; i ++)
	{
		set_servo_speed(i, 150);
		set_servo_target(i, NEUTRAL_SERVO_POSITION);
		servoPositions[i] = NEUTRAL_SERVO_POSITION;
	}

	printServoStatus(servo, servoPositions[servo]);
 
    while(1)
    {

		// top button changes servo
		if(button_is_pressed(TOP_BUTTON))
		{
			servo ++;
			if(servo >= NUM_SERVOS)
			{
				servo = 0;
			}
			wait_for_button_release(TOP_BUTTON);
			printServoStatus(servo, servoPositions[servo]);
		}
		
		// middle button increases servo position
		if(button_is_pressed(MIDDLE_BUTTON))
		{
			wait_for_button_release(MIDDLE_BUTTON);
			servoPositions[servo] += 100;
			if(servoPositions[servo] > MAX_SERVO_POSITION)
			{
				servoPositions[servo] = MAX_SERVO_POSITION;
			}
			set_servo_target(servo, servoPositions[servo]);
			printServoStatus(servo, servoPositions[servo]);
		}
		
		// bottom button decreases servo position
		if(button_is_pressed(BOTTOM_BUTTON))
		{
			wait_for_button_release(BOTTOM_BUTTON);
			servoPositions[servo] -= 100;
			if(servoPositions[servo] < MIN_SERVO_POSITION)
			{
				servoPositions[servo] = MIN_SERVO_POSITION;
			}
			set_servo_target(servo, servoPositions[servo]);
			printServoStatus(servo, servoPositions[servo]);
		}

    }
}

The second solution is better anyways, but I’m still curious as to why the first one doesn’t really work.

Thanks,
Geoff

4

Thanks for tracking this down further. I think I understand the source of the problem now. From the OrangutanLCD library:

// Send either data or a command
void OrangutanLCD::send(unsigned char data, unsigned char rs, unsigned char numSends)
{	

...  (I removed some irrelevant code here)

	// Save our DDR and port information
#if defined(_ORANGUTAN_SVP) || defined(_ORANGUTAN_X2)
	unsigned char temp_ddrc, temp_portc;
	temp_ddrc = DDRC;
	temp_portc = PORTC;
  ...
#endif

... (more irrelevant stuff removed)

	// Restore our DDR and port information
#if defined(_ORANGUTAN_SVP) || defined(_ORANGUTAN_X2)
	DDRC = temp_ddrc;
	PORTC = temp_portc;
  ...
#endif
}

The send() function is used by every other LCD command, and as you can see, it first stores the state of the pins used by the LCD and then restores them when it’s done. This was an attempt to allow the LCD pins to be used for some other purposes with minimal disturbance from the LCD, and to save time, we save and then later restore the entire port register rather than just the pins specifically used by the LCD. This is the part that I believe is causing you trouble. Every once in a while, the interrupt service routine that makes the OrangutanServo library work will fire between the save and restore and change the state of pin C0. Then the LCD send routine promptly overwrites that change.

I think the easiest workaround for you at this point would be to replace pin C0 with any other pin that is not on port C. A more robust fix would involve making the save and restore part of the LCD code only act on those pins specifically used by the LCD rather than the whole port, which I will add to our to-do list. If you use something other than C0, does the jitter go away?

- Ben

5

Hi Ben,

I’ll test your suggestion later tonight; stay tuned.

Geoff

6

Hi Ben,

Well, it was definitely C0. Using that pin with the earlier version of my program caused the jitter, but the jitter stopped when I switched to D0 instead.

Hope that helps! Thanks again for your assistance.

Geoff

7

I’m glad that fixed it; thanks for letting us know!

- Ben