help building pid controller

Go To Last Post
2 posts / 0 new
Author
Message
#1
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0
#include "complementary_filter.h"

 uint8_t MPU6050_data[12];
volatile int acx,gyy,acx_lut_index,gyy_lut_index,acx_deg,gyy_deg,anglex;

void COMPFILT_Initialize()
{
	TCCR2A = (WGM21);
	//CTC 0 TO OCR2A
	TCCR2B = (CS22) | (CS21) | (CS20);
	//1024 PRESCALER
	
	OCR2A = 10;
	//1562.5 hz sample rate
	TIMSK2 = (1 << OCIE2A);
	//OCR2A COMAPARE INTTERUPT
}

ISR(TIMER2_COMPA_vect)
{
	MPU6050_ReadByteS(ACXH,&MPU6050_data[0],6);
	MPU6050_ReadByteS(GYXH,&MPU6050_data[6],6);
	
	acx = MPU6050_data[0] << 8 | MPU6050_data[1];
	gyy = MPU6050_data[8] << 8 | MPU6050_data[9];
	
	//-----------------------------------------------------------------------------------------------------------------------
	
	if (acx < 0)
		acx_lut_index = -acx;
	else
		acx_lut_index = acx;
	
	acx_lut_index = acx_lut_index >> 6;
	acx_lut_index = acx_lut_index << 1;
	acx_deg = (pgm_read_byte(&arcsinelut[acx_lut_index]) << 8 | pgm_read_byte(&arcsinelut[acx_lut_index + 1]));
	
	if (acx < 0)
		acx_deg = -acx_deg;
//----------------------------------------------------------------------------------------------------------------------------	
	if (gyy < 0)
		gyy_lut_index = -gyy;
	else
		gyy_lut_index = gyy;
	
	gyy_lut_index = gyy_lut_index >> 7;
	gyy_lut_index = gyy_lut_index << 1;
	gyy_deg = (pgm_read_byte(&gyrolut[gyy_lut_index]) << 8 | pgm_read_byte(&gyrolut[gyy_lut_index + 1]));
	
	if (gyy < 0)
		gyy_deg = -gyy_deg;
	//-------------------------------------------------------------------------------------------------------------------------
	
	anglex = multfix(anglex,0x00fd) + gyy_deg + multfix(acx_deg,0x0003);//00fb,0005//00fd,0003
	//--------------------------------------------------------------------------------------------------------------------------
	
	int speed,error,sum_error = 0,previous_error = 0,p_term,i_term,d_term;
	//--------------------------------------------------------------------------------------------------------------------------
	
	error = anglex - SETPOINT;
	//---------------------------------------------------------------------------------------------------------------------------
	if (error > P_ERROR_MAX)
	{
		p_term = PID_MAX_OUTPUT;
	} 
	else if(error < -P_ERROR_MAX)
	{
		p_term = -PID_MAX_OUTPUT;
	}
	else
	{
		p_term = multfix(anglex,KP);
	}
	//---------------------------------------------------------------------------------------------------------------------------
	
	if (sum_error + error > PID_MAX_OUTPUT)
	{
		sum_error = PID_MAX_OUTPUT;
		i_term = PID_MAX_OUTPUT;
	} 
	else if (sum_error + error < -PID_MAX_OUTPUT)
	{
		sum_error = -PID_MAX_OUTPUT;
		i_term = -PID_MAX_OUTPUT;
	}
	else
	{	
		if (sum_error + error > I_ERROR_MAX)
		{
			i_term = PID_MAX_OUTPUT;
		} 
		else if (sum_error + error < -I_ERROR_MAX)
		{
			i_term =  PID_MAX_OUTPUT;
		}
		else
		{
			i_term = multfix(KI,sum_error);	
		}
		
		sum_error = sum_error + error;
	}
	//-------------------------------------------------------------------------------------------------------------------------------
	if ((error - previous_error > PID_MAX_OUTPUT) || (error - previous_error > I_ERROR_MAX))
	{
		d_term = PID_MAX_OUTPUT;
	} 
	else if((error - previous_error < -PID_MAX_OUTPUT) || (error - previous_error < -I_ERROR_MAX))
	{
		d_term = -PID_MAX_OUTPUT;
	}
	else
	{	
		d_term = multfix(error-previous_error,KD);
	}
	previous_error = error;
	//--------------------------------------------------------------------------------------------------------------------------------
	
	if (p_term + i_term + d_term > PID_MAX_OUTPUT)
	{
		speed = PID_MAX_OUTPUT;
	} 
	else if (p_term + i_term + d_term < -PID_MAX_OUTPUT)
	{
		speed = -PID_MAX_OUTPUT;
	}
	else
		speed = p_term + i_term + d_term;
    //-------------------------------------------------------------------------------------------------------------------------------
	if (speed < 0)
	{
		speed = -speed;
		speed = speed << 1;
		MOTOR_Backward((uint8_t)(speed >> 8),(uint8_t)(speed >> 8));
		
	} 
	else
	{
		speed = speed << 1;
		MOTOR_Forward((uint8_t)(speed >> 8),(uint8_t)(speed >> 8));
	}
		
}

can someone take a look at the code and tell if my implemention of pid limits is correct or not

Last Edited: Sat. Jan 18, 2020 - 12:11 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

You've already got a thread on this:  https://www.avrfreaks.net/forum/help-pid-code

 

Are you just ignoring all the replies there?

 

Top Tips:

  1. How to properly post source code - see: https://www.avrfreaks.net/comment... - also how to properly include images/pictures
  2. "Garbage" characters on a serial terminal are (almost?) invariably due to wrong baud rate - see: https://learn.sparkfun.com/tutorials/serial-communication
  3. Wrong baud rate is usually due to not running at the speed you thought; check by blinking a LED to see if you get the speed you expected
  4. Difference between a crystal, and a crystal oscillatorhttps://www.avrfreaks.net/comment...
  5. When your question is resolved, mark the solution: https://www.avrfreaks.net/comment...
  6. Beginner's "Getting Started" tips: https://www.avrfreaks.net/comment...
Topic locked