help building self balancing robot

Go To Last Post
6 posts / 0 new
Author
Message
#1
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

the processors seems to be getting stuck the essential code for filter and pid is attached below

#include "complementary_filter.h"

uint8_t MPU6050_RawData[12];
volatile float acx,gyy,anglex = 0,error,setpoint = 8,kp = 9,kd = 0.15,ki = 10,previous_error = 0,sum_error = 0,dt = 0.00064,p_term,i_term,d_term,pid_total;

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_RawData[0],6);
	MPU6050_ReadByteS(GYXH,&MPU6050_RawData[6],6);
	acx = (360.0/3.14159265)*asin((MPU6050_RawData[0] << 8 | MPU6050_RawData[1])/32768.0);
	gyy = 250.0*((MPU6050_RawData[8] << 8 | MPU6050_RawData[9])/32768.0);
	anglex = 0.98*(anglex + gyy*dt) + 0.02*acx;
	//-----------------------------------------------------------------------------
	error = setpoint - anglex;
	sum_error = sum_error + error;
	p_term = kp*error;
	i_term = ki*sum_error*dt;
	d_term = kd*(error - previous_error)/dt;
	previous_error = error;
	//-----------------------------------------------------------------------------
	if (p_term >= 255.0)
	{
		p_term = 255.0;
	}
	else if (p_term <= -255.0)
	{
		p_term = -255.0;
	}
	//-------------------------------------------------------------------------------
	if ((i_term > 0.0 && error < 0.0) || (i_term < 0.0 && error > 0.0))
	{
		i_term = 0.0;
		sum_error = 0.0;
	}

	if(sum_error >= 255.0/(ki*dt))
	{
		sum_error = 255.0/(ki*dt);
	}else if (sum_error <= -255.0/(ki*dt))
	{
		sum_error = -255.0/(ki*dt);
	}
	//---------------------------------------------------------------------------------
	if (d_term >= 255.0)
	{
		d_term = 255.0;
	}
	else if (d_term <= -255.0)
	{
		d_term = -255.0;
	}
	//----------------------------------------------------------------------------------
	pid_total = p_term + i_term + d_term;
	if (pid_total >= 255.0)
	{
		pid_total = 255.0;
	}
	else if (pid_total <= -255.0)
	{
		pid_total = -255.0;
	}
	//-----------------------------------------------------------------------------------
	if (pid_total >= 0.0)
	{
		MOTOR_Backward((uint8_t)pid_total,(uint8_t)pid_total);
	}else
	{
		pid_total = -pid_total;
		MOTOR_Forward((uint8_t)pid_total,(uint8_t)pid_total);
	}
}

 

Last Edited: Fri. Jan 31, 2020 - 04:59 PM
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 1

jeet55 wrote:
getting stuck
Not much of an error report is it? "stuck" in computing terms often means a conditional loop in which the escape condition is never actually met (often unexpectedly) so exactly where do you think execution is "stuck" and how are you observing this? If it's using an OCD/ICE link like JTAG or something then surely the debugger that shows you where it is stuck can also reveal WHY it is stuck?

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Lots of floating-point maths and serial (I2C?) comms in an ISR is not often a great idea ...

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...
  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

Can you explain why?

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

jeet55 wrote:
Can you explain why?
How much time are you expecting to spend inside the ISR? How long does the FP calculation take? How long for the I2C activity? What happens if an interrupt is missed?

 

Can the robot get back up? cheeky

Ross McKenzie, Melbourne Australia

  • 1
  • 2
  • 3
  • 4
  • 5
Total votes: 0

 

What makes you say "stuck"  Does the IRQ no longer fire?

 

Try removing these calls & see if it still is hung up (see if calculations are still repeating & finishing)..maybe one of the calls is not returning

Note these calls must not be called from anywhere else , unless they are reentrant capable.

MPU6050_ReadByteS
MOTOR_xxxx ((uint8_t)pid_total,(uint8_t)pid_total);

When in the dark remember-the future looks brighter than ever.   I look forward to being able to predict the future!