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.