Hello Freaks,
I'm experiencing some strange problem on a project that I'm working on with an atmega328p.
I've configured timer1 as CTC mode with a 256 prescaller to generate the desired frequency to send to a stepper motor witch is given by a gyroscope angle (MPU6050) to be processed by PID controller implemented on while loop.
The problem is that the timer1 stops one second from time to time (not always at the same frequency, random like you can see on attached image) without any apparent reason.
Attached the code bellow:
/********************************************************
Language : C
Year : 2019/2020
Class : LABSI
Authors : Luís Silva | João Loureiro
E-mail : 1101420@isep.ipp.pt | 1131109@isep.ipp.pt
*********************************************************/
#define F_CPU 16000000UL /* Define CPU clock Frequency e.g. here its 16MHz */
#include <avr/io.h> /* Include AVR std. library file */
#include <avr/interrupt.h> /* Include AVR interrupt library file */
#include <util/delay.h> /* Include delay header file */
#include <inttypes.h> /* Include integer type header file */
#include <stdlib.h> /* Include standard library file */
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#include "MPU6050_res_define.h" /* Include MPU6050 register define file */
#include "I2C_Master_H_file.h" /* Include I2C Master header file */
//unsigned char PWM=0;
volatile unsigned int c = 0;
char float_[10];
char TxBuffer[50];
float Acc_x,Acc_y,Acc_z,Temperature,Gyro_x,Gyro_y,Gyro_z;
float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];
float elapsedTime;
float rad_to_deg = 180/3.141592654;
//volatile float PWM;//, error, previous_error;
//#define duty(val) (255*val)/100 // % Duty Cicle
//#define dutym(valbom) (-67*valbom+1000)
/* Functions */
void inic()
{
DDRC = (1<<DDC0) | (1<<DDC1); //0x0F; // Set PC0 & PC1 as output to Motor direction & scope test probes
DDRB = 0b00000110; // Set PB1 and PB2 as output OC1A & OC1B
// DDRD = (1<<DDD6) | (1<<DDD5); // Set PD5 & PD6 as output OC0A to test TIMER0
/* Timer 0 */
/* TCCR0A = (1<<COM0A1) | (1<<COM0B1)| (1<<WGM01) | (1<<WGM00) ; // Clear OC0A | Fast PWM
TCCR0B = (1<<CS02) | (1<<CS00); // PRESCALER=1024
TIMSK0 |= (1<<TOIE0); // Overflow interrupt enable
OCR0A=0; // PWM = 0
OCR0B=0; // PWM = 0
*/
/* Timer 1 toggle LED*/
TCCR1A = (1 << COM1A0) | (1 << COM1B0); // Enable OC1A e OC1B to motors
TCCR1B = (1 << CS12) | (1 << WGM12); // CLKIO/256 (From prescaler) | Mode CTC
// TIMSK1 |= (1 << OCIE1A); // Enable TIMER1
OCR1A = 62535; // motor speed right
OCR1B = 62535; // motor speed left
/* Timer 2 */
TCCR2A = (1<<WGM21); // MODE CTC
TCCR2B = (1 << CS21) | (1 << CS22); // CLKIO/256 (From prescaler)
TIMSK2 |= (1 << OCIE2A); // Enable TIMER0
OCR2A = 88; // ~0.001088s ~900Hz 68
/* USART */
/* UBRR0H=0;
UBRR0L=51; // BAUDRATE 38400
UCSR0A=(1<<U2X0); // Double Speed
UCSR0B=(1<<RXCIE0)|(1<<RXEN0)|(1<<TXEN0); // RX Complete Interrupt Enable, Receiver Enable; transmite Enable
UCSR0C=(1<<UCSZ01)|(1<<UCSZ00); // 8 bits
*/
sei();
// sprintf(TxBuffer, "Init successfully!!!\r\n");
// SendMessage(TxBuffer);
}
void MPU6050_Init() /* Gyro initialization function */
{
_delay_ms(250); /* Power up time >100ms */
I2C_Start_Wait(0xD0); /* Start with device write address */
I2C_Write(SMPLRT_DIV); /* Write to sample rate register */
I2C_Write(0x07); /* 1KHz sample rate */
I2C_Stop();
I2C_Start_Wait(0xD0);
I2C_Write(PWR_MGMT_1); /* Write to power management register */
I2C_Write(0x01); /* X axis gyroscope reference frequency */
I2C_Stop();
I2C_Start_Wait(0xD0);
I2C_Write(CONFIG); /* Write to Configuration register */
I2C_Write(0x00); /* Fs = 8KHz */
I2C_Stop();
I2C_Start_Wait(0xD0);
I2C_Write(GYRO_CONFIG); /* Write to Gyro configuration register */
I2C_Write(0x18); /* Full scale range +/- 2000 degree/C */
I2C_Stop();
I2C_Start_Wait(0xD0);
I2C_Write(INT_ENABLE); /* Write to interrupt enable register */
I2C_Write(0x01);
I2C_Stop();
}
void MPU_Start_Loc()
{
I2C_Start_Wait(0xD0); /* I2C start with device write address */
I2C_Write(ACCEL_XOUT_H); /* Write start location address from where to read */
I2C_Repeated_Start(0xD1); /* I2C start with device read address */
}
void Read_RawValue()
{
MPU_Start_Loc(); /* Read Gyro values */
Acc_x = (((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Ack());
Acc_y = (((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Ack());
Acc_z = (((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Ack());
Gyro_x = (((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Ack());
Gyro_y = (((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Ack());
Gyro_z = (((int)I2C_Read_Ack()<<8) | (int)I2C_Read_Nack());
I2C_Stop();
}
void AngleCalc()
{
//---ACC X---
// Acceleration_angle[0] = atan((Acc_y/16384.0)/sqrt(pow((Acc_x/16384.0),2) + pow((Acc_z/16384.0),2)))*rad_to_deg;
//---ACC Y---
Acceleration_angle[1] = atan(-1*(Acc_x/16384.0)/sqrt(pow((Acc_y/16384.0),2) + pow((Acc_z/16384.0),2)))*rad_to_deg;
//---GYRO X---
// Gyro_angle[0] = Gyro_x/131.0;
//---GYRO Y---
Gyro_angle[1] = Gyro_y/131.0;
elapsedTime = 0.001408; //tempo relativo à frequência de aquisição TIMER0
//Filtro complementar
//---X axis angle---
//Total_angle[0] = 0.98 *(Total_angle[0] + Gyro_angle[0]*elapsedTime) + 0.02*Acceleration_angle[0];
//---Y axis angle---
Total_angle[1] = 0.98 *(Total_angle[1] + Gyro_angle[1]*elapsedTime) + 0.02*Acceleration_angle[1];
}
float sat(float _pid, int umin, int umax)
{
if (_pid <= umin)
{
_pid = umin;
}
if (_pid >= umax)
{
_pid = umax;
}
return _pid;
}
void PID()
{
float desired_angle = 0;
float error = Total_angle[1] - desired_angle;
//Proporcional
float kp = 0.005; //Com este valor(0.04), só com a componente P, os motores (quase) não param, mas é sempre a dar brita!!
if (kp < 0) kp = 0;
float pid_p = kp * error;
//Integral
float ki = 0.0007; // Na casa dos 0.000K (acho eu)
if (ki < 0) ki = 0;
static float pid_i;
pid_i += error * ki;
pid_i = sat(pid_i,-100,100);
// if (pid_i > 1000) pid_i = 1000; //Limite wind-up
// if (pid_i < -1000) pid_i = -1000;
//Derivativo
static float previous_error = 0;
float kd = 1;
if (kd < 0) kd = 0;
float pid_d = (error - previous_error) * kd;
previous_error = error;
float pid_tot = pid_p + pid_i + pid_d; // Calcular saída temporária
// PWM = sat(pid_tot,-20,20); // saturação do atuador
if (error < 0)
{
PORTC = 0b00000000;
// OCR0A = duty(abs(PWM)); //LED para simular PID
// OCR0B = duty(0); //LED para simular PID
OCR1B = abs(pid_tot);
OCR1A = abs(pid_tot);
}
if (error > 0)
{
PORTC = 0b00000011;
// OCR0A = duty(0); //LED para simular PID
// OCR0B = duty(abs(PWM)); //LED para simular PID
OCR1B = abs(pid_tot);
OCR1A = abs(pid_tot);
}
if (error == 0)
{
PORTC = 0b00000000;
OCR1B = 500; // Supostamente perto de parado
OCR1A = 500; // Supostamente perto de parado
}
}
int main()
{
inic(); // Main configurations
I2C_Init(); // Initialize I2C
MPU6050_Init(); // Initialize MPU6050
while(1)
{
if (c==1)
{
Read_RawValue(); // Read gyro and acc from MPU6050
AngleCalc();
PID();
c=0;
}
}
}
/* Interrupts */
ISR(TIMER2_COMPA_vect)
{
if (c==0)
{
c=1;
}
}
Thanks in advance for your help.
Best Regards.
) when you came to Portugal