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

[Alan Anderson]

The algorithm and implementation look great. There's a bit of confusion in the comments, where it misrepresents the equation as being based only on the error and not on the derivative or integral, but the code does what I'd expect from a full PID controller. (Or it would, if you hadn't commented out the line doing the I accumulation. You should probably say something explicit about having removed it, in order not to confuse anyone.)

One other thing is completely unrelated to what the code is doing, and likely won't cause problems, but still ought to be corrected as soon as possible. You're treating 255 as the highest possible pwm value. In the IFI system, the maximum should instead be 254. Where you "invert" the right side output value, you similarly need to subtract from 254 instead of 255 -- that way, the 127 "neutral" value remains as it is.

[tdlrali]

Looks great!

Hey Alan, is 254 really the max in the IFI setup? I always thought 255 was the max... I think I incorrectly corrected a few people if that is the case.

[DonRotolo]

I would be interested in a photo or description of your drive setup, so I can make better sense of the code.

Nontheless, I very much like your implementation - looks nice and clean, and hope to one day be able to make use of it as a teaching tool. Once I really understand it, that is...

Don

[Bongle]

Originally Posted by tdlrali

Looks great!

Hey Alan, is 254 really the max in the IFI setup? I always thought 255 was the max... I think I incorrectly corrected a few people if that is the case.

255 can do some wierd stuff. Way back in 2003, I couldn't figure out why our robot wouldn't drive straight. Another team pointed out that 255 was some sort of null character. I changed it to 254, and the thing started driving perfectly.

The X axis sets the "desired" heading.

Last year we found that our robot could massively out-spin the maximum rotation rate supported by the gyro. What do you do once the gyro reading goes 'beyond' the maximum/minimum of the chip?

[Render]

one way you could improve your code is remove the floats since they are very expensive to the processor and replace them with a numerator and denominator. Then when doing your math you do everything for that portion and divide last.

ex.
char Kp = 5;
char kpDiv = 100;

pTerm = (Kp * p_error) / KpDiv;

You will lose the precision of having a floating point value in the end, but with PWM values I do not think that matters anyways

also I am wondering why your errors are floats?

[adamdb]

Alan,
I did sort of say we weren't using the I component by saying we were using P&D, but should have been a little more clear. I took a pass through the comments, but as usual missed a few that no longer matched up with the code. Odd that you have had issues with 255, we have always used it without issues.

Don,
The drive setup is a typical setup with one motor on each side (facing opposite directions) that power the drive wheels on that side. As the motors face opposite directions we need to do the inversion for (in our case) the right side as shown in the bottom of the PID_adjust() routine.

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

I definitely urge you to play with the gyro and this code and the other available examples for driving the robot. We hadn't used a gyro in previous years because I didn't understand them or PID routines, but dug into it this year and did some reading and the result has been great.

Render,
As I mentioned in the original post the code was not optimized for size or speed and we will get rid of the floats in future revisions. The errors were made floats so it didn't have to add code to convert from an integer to floating point before it made the calculations.

Thanks all for the feedback!
Adam

[Uberbots]

your method is very neat indeed, but I still like my little PID engine better

output = pid_control(&part, error);

[Tom Bottiglieri]

We did something cool with our PID stuff this year. We wanted to get an OOP type feel, but we're forced to use the next best thing, structures and private functions.

It pretty much consisted of making a PID structure which contained everything we needed to know about the system (gains, integral bounds, etc.). We would make a new instance of that structure, and pass a pointer of that struct into different functions to set the gains and compute the output values. I believe this is what Uberbots did.

[adamdb]

Uberbots, Tom,
We only needed the PID for our drive system so we didn't go to the effort of making it generic and passing in all the relevant parameters (which is a VERY slick way to do it). I would love to see the code you used so we can learn from it. Have you posted it in another thread somewhere?

Thanks

[Tom Bottiglieri]

Originally Posted by adamdb

Uberbots, Tom,
We only needed the PID for our drive system so we didn't go to the effort of making it generic and passing in all the relevant parameters (which is a VERY slick way to do it). I would love to see the code you used so we can learn from it. Have you posted it in another thread somewhere?

Thanks

Nope, but I'll put it up later today. Off to class!

[Alan Anderson]

Originally Posted by tdlrali

Hey Alan, is 254 really the max in the IFI setup?

The proper maximum pwm output is indeed 254. Here is one bit of documentation to that effect:

Originally Posted by IFI Tech Support

In the Robot Controller program, a value of 0 corresponds to a 1ms pulse. A 1.5ms pulse means neutral, or a value of 127, while a 254 corresponds to a 2ms pulse.

See also this post on the IFI Technical Support forum, which shows the range 231-254 as "Full Forward" on a Victor.

Originally Posted by Bongle

255 can do some wierd stuff. Way back in 2003, I couldn't figure out why our robot wouldn't drive straight. Another team pointed out that 255 was some sort of null character. I changed it to 254, and the thing started driving perfectly.

The communication protocol between OI and RC uses a pair of 255 (0xFF) characters to synchronize frames of data. Back in the pre-C days, it might have been possible to confuse it by setting adjacent pwm values to 255. My experience is with the 2004 and later control system, which refuses to report a pwm value greater than 254. I don't know whether the limit is enforced on the actual pwm output or if it's just the dashboard data coming back from the RC that won't go that high.

[adamdb]

Tom,
I saw your post of your PID code, thanks! Reviewing this with my programming students will be a great learning exercise as we haven't covered structures yet.

Alan,
Thanks for the info on the max value of 254. Looks like we need to go through our code and make some changes.

[Tom Bottiglieri]

Originally Posted by adamdb

Tom,
I saw your post of your PID code, thanks! Reviewing this with my programming students will be a great learning exercise as we haven't covered structures yet.

That was Billy's from 1124. Either way, its a pretty good way to get into structures. It covers typedefs, local declarations, and pointers.

[artdutra04]

Originally Posted by Bongle

Last year we found that our robot could massively out-spin the maximum rotation rate supported by the gyro. What do you do once the gyro reading goes 'beyond' the maximum/minimum of the chip?

Team 228 purchased an additional 300 degree/second gyro for use with PID control with our drivetrain, as this proved much more adequate for drive-train use than the Kit of Parts gyro. So we used the stock KoP gyro for PID control on our arm instead.