Gyro PID User drive - How we did it

[adamdb]

Thought I would share how we are using the Gyro and a PID routine to help drive our robot.

#include <stdio.h>  
  
#include "ifi_aliases.h"  
#include "ifi_default.h"  
#include "ifi_utilities.h"
#include "rv_setup.h"
#include "timer.h"
#include "gyro.h"
#include "serial_ports.h"

extern int init_gyro( void );
extern void print_gyro( void );

static int left_out=127 , right_out=127;

static int counter = 0;
static char state = 0;
static int i = 0;
static int x_joy_adjust = 0;
static int y_joy_adjust = 0;
static int last_speed = 127;


int abs( int arg )
{
	if (arg >= 0)
		return (arg);
	else
		return(arg * -1);
}

int adjust_speed( int desired, int current )
{
	int diff;
	
	diff = desired - current;
	
	if ( abs(diff) <= 10 )
	{
		if ( diff < 0 )
			diff = -1;
		else if ( diff > 0 )
			diff = 1;
	}
	else
		diff /= 10;
		
	return(diff);
}

/*---------------------------------------------------------
	PID_adjust()
	Adjusts motor outputs based on a slightly non-typical 
	PID calculation.
	
	Full equation is:
	
	adjust = Kp * error - Kd * error + Ki * error
	
	For complete explanation of this PID routine see the
	excellent technical paper on Motion Control by
	Larry Barello at:
	http://www.barello.net/Papers/Motion_Control/index.htm
  -------------------------------------------------------*/
void PID_adjust( int heading, int speed )
{
	int current_heading;
	int temp_gyro_angle;
	float p_error = 0.0;
	float i_error = 0.0;
	float d_error = 0.0;
	static float prev_error = 0.0;
	int output;
	const float Kp = 0.05;
	const float Kd = 0.05;
	const float Ki = 0;
	
	// First copy the forward/back speed to BOTH outputs
	last_speed += adjust_speed( speed, last_speed );
	left_out = right_out = last_speed;
	
	// Get the current gyro angle
	temp_gyro_angle = Get_Gyro_Angle();
	
	p_error = (heading - temp_gyro_angle);
	d_error = p_error - prev_error;
//	i_error += p_error;
	
	output = ( Kp * p_error - Kd * d_error + Ki * i_error );

	prev_error = p_error;

	left_out -= output;
	right_out += output;
	
	// Make sure the output values don't exceed the limits
	if ( left_out < 0 )
		left_out = 0;
	else if ( left_out > 255 )
		left_out = 255;

	if ( right_out < 0 )
		right_out = 0;
	else if ( right_out > 255 )
		right_out = 255;
		
	printf( "left: %d  rght: %d\r\n", left_out, right_out );
	txdata.user_byte3 = left_out;
	txdata.user_byte4 = right_out;

	// Invert the right output and put them to the wheels
	OUT_PWM_LM = left_out;
	OUT_PWM_RM = 255 - right_out;
}

/*---------------------------------------------------------
	new_drive()
	
	New scheme using gyro for heading stability.
	
	Y joystick controls speed of fore and aft
	movement
	
	X joystick controls the desired heading.
	
	Proportional part of PID loop adjusts x & y portion of
	motor output to adjust motors to turn to the desired
	heading.
  ---------------------------------------------------------*/
void new_drive( void )
{
	int y_axis, x_axis;
	static int desired_heading = 0;
	
	y_axis = IN_PWM_Y;
	y_axis += y_joy_adjust;		// add in autotrim value
	
	// Now read the X axis
	x_axis = 255 - IN_PWM_X;
	x_axis -= 127;				// convert to range of -127 to +127
	x_axis -= x_joy_adjust;		// add in autotrim value
	
	desired_heading += x_axis / 5;
	
	// Limit it to reasonable values
	if ( desired_heading <= -3600 || desired_heading >= 3600 )
	{
		desired_heading = 0;
		Reset_Gyro_Angle(); 
	}
		
	printf( "y: %d  x: %d   h: %d  ", y_axis, x_axis, desired_heading );
	
	PID_adjust( desired_heading, y_axis );
}

/*---------------------------------------------------------
	This routine reads whatever the current "center" point
	of the X and Y axis on the joystick is and uses that as
	the center from now on.
  ---------------------------------------------------------*/
void auto_trim( void )
{
	x_joy_adjust = IN_PWM_X;
	x_joy_adjust = 127 - x_joy_adjust;
	y_joy_adjust = IN_PWM_Y;
	y_joy_adjust = 127 - y_joy_adjust;
}

void normal_drive( void )
{
	static int state = 0;
	
	switch (state)
	{
		case 0:		// initialize gyro
			auto_trim();
			if ( init_gyro() )
				state++;
			break;
			
		case 1:
			new_drive();
			break;
	}
}

Some significant points:

We are using a single joystick to drive our robot. The Y axis controls the forward or reverse speed. The X axis sets the "desired" heading. The gyro and PID routines are used to turn the robot from the "current" heading to the "desired" heading.

Some significant routines:

normal_drive() - this is the entry point and would be called from user_routines.c

auto_trim() - this function reads the current values of the joysticks, subtracts them from 127 and uses this as the "center" value. It is called once when the user drive code starts. So no matter where the trims are when the robot starts, the motors start at "off". We were constantly fiddling with the trims to get the robot to not move when turned on. This adjustment eliminates this issue.

adjust_speed() - this function takes the "desired" speed and the last_speed output to the motors and "ramps" the speed up and down rather than allowing instant changes. This helps to save the transmission from sudden direction changes.

new_drive() - this routine reads the X and Y axis from the joystick and sets the forward/backward speed based on the Y axis and modifies the "desired" heading based on the X axis reading.

PID_adjust() - this is where the magic happens. We are using a slightly non-standard PID calculation from Larry Barello (link in the code comments above). We are actually only using the P & D elements as this was sufficient for us. The additions for the I element are in the code, but commented out. This routine calculates the error between the "desired" heading and the current heading read from the gyro and calculates the value to be sent out to the motors.

Note This routine has not been optimized for size or speed. We do use floating point variables and calculations, but once you determine the constants to use for the Kx constants it would be fairly easy to modify to use integer variables and calculations.

I hope this of interest and welcome any feedback.

Adam Bryant
Programming Mentor
Team 1583
Ridge View Academy Rambotics