My error derivative is going crazy

I’m trying to get PID control for the gyro working, and so far P is working fine. The D, however, is going to unreasonable values because my angle_err_last variable doesn’t work at all. Here’s my teleop.c function, sans gyro bias calc and printfs. I’m using the new code with C18 3.10.

void Teleop(void)
{
	#define KP 20/100;
	#define KD 0/10

	static unsigned int i = 0;
	static unsigned int j = 0;
	int temp_gyro_rate;
	long temp_gyro_angle;
	int temp_gyro_bias;
	long angle_err;
	long angle_err_last;
	long angle_err_d;
	long heading = 1800;
	long turn;

	i++;
	j++; // this will rollover every ~1000 seconds

             ...

		temp_gyro_bias = Get_Gyro_Bias();
		temp_gyro_rate = Get_Gyro_Rate();
		temp_gyro_angle = Get_Gyro_Angle();

	if(j >= 120)
	{
		temp_gyro_angle = gyro_cycle(temp_gyro_angle);
		angle_err = (heading - abs(temp_gyro_angle));

		angle_err_d = (angle_err - angle_err_last);

		

		turn = angle_err * KP + angle_err_d * KD;

		motor_left = limit(127 + turn);
		motor_right = limit(127 + turn);

	}


             ...


	angle_err_last = angle_err;

	Update_OI_LEDs();	// located in ifi_code.c
}

The P correction doesn’t know left from right at the moment, but I’m not worried about that at the moment.

I set all of the variables as long to avoid type mismatches. The angle_err_last variable jumps all over the place, never coinciding with what angle_err actually was. Anyone know why?

edit: I commented out everything touching angle_err_last except for the declaration, and it still went all over the place.

angle_err_last should be static.

It worked. Thank you.

I am not sure if this applies to your case, but if you’re integrating the gyro output to get angular position, and then differentiating it to get the change in angular position, wouldn’t it be more efficient to just scale the rate output of the gyro? I think that you are differentiating the error though, I am not sure if this has any advantage. From my understanding, the D term of a PID loop is to slow down as you approach the goal, to reduce overshooting/oscillations.