Hello Freaks!
I am in the last stanges of making my first auto balancing robot. It is made out of 2 stepper motors, 2 stepper motor drivers, mpu6050 and ATmega8A as brain.
I have already achieved balancing however I am currently trying to achieve control from phone by bluetooth (HC05). Here is short vid:
https://www.youtube.com/watch?v=...
To make it possible I need to be able to control 2 stepper motors simultaneously (so they can rotate at different speed when robot turn.
currently I control them by using ISR. Algorithm is simple:
1. Mpu6050 reads gyro and accel
2. Complementary filter calculates current angle
3. PID calculates required acceleration
4. currentSpeed = reqAccel*dt + currentSpeed;
5. calculate required freqency of 0,1 signal generated on I/O port to achieve currentSpeed (stepper motor driver makes motor make step each time 0 changes to 1)
6. generate interrupts that change state of the port at required freqency. (CTC)
on a single timer (timer2) this works perfectly as we can see on a video. However I have found it difficult to do the same thing when using 2 timers (one for each motor) simultaneously, so when, for example, I send from smartphone 'turn' signal which is simply difference between rotation speed of two motors ( half of this difference should be added to motor1 speed and half should be deducted from motor2 speed) I have 2 different interrupt frequencies one for each motor.
My tests prove that on small frequencies this routine works, however over certain frequency program just stopps working.
So there is time for some codes. I will paste just the interrupt test codes to keep this post as clear as possible.
interrupt test is aplication sending letters with bluetooth from 3 different sources.
- '_' is sent with main loop of program
- 'A' is sent with TIMER1 CTC interrupt
- 'B' is sent with TIMER2 CTC interrupt
#ifndef F_CPU
#define F_CPU 4210000UL
#endif
#include <avr/io.h>
#include "USART.h"
#include <util/delay.h>
#include <avr/interrupt.h>
double extraTime1=0;
double extraTime2=0;
int main(void)
{
//TIMER2
TIMSK |= (1 << OCIE2); //Output Compare Interrupt Enable 2
TCCR2 |= (1 << WGM21) | (1 << CS22); //CTC, prescaler 64
OCR2 = 125;
sei();
//TIMER1
_delay_ms(1);
TCCR1B |= (1<<CS11)|(1<<CS10)|(1 << WGM12); //CTC, prescaler 64
OCR1A = 125;
TIMSK |= (1 << OCIE1A); //Output Compare Interrupt Enable 1A
sei();
usart_Init();
while (1) {
_delay_ms(1000);
UDR = '_';
}
}
ISR(TIMER1_COMPA_vect){
extraTime1++;
if(extraTime1 >= 526){ //occurs every ~1 s
UDR = 'A';
extraTime1=0;
}
}
ISR(TIMER2_COMP_vect){
extraTime2++;
if(extraTime2 >= 526){ ///occurs every ~1 s
UDR = 'B';
extraTime2=0;
}
}
Result is 3 signs every 1 second (sometimes I lose '_' which is acceptable for me at this rate as PID control is rather robust and could probably handle this)
when I increase interrupt frequency for both timers:
OCR2 = 25; //frequency increased 5 times
OCR1A = 25;
if(extraTime1 >= 2630){ //increased 5 times so sign is sent still every 1s
if(extraTime2 >= 2630){Result is 'rather unacceptable' (too often I lose main loop of program):
and finally when I again increase frequency 5 times the result is absolutely unaccaptable.
Additional test I made includes 1 timer (timer2) but with increased frequency AGAIN 5 times
I receive just B letter from time to time (not in 1s delay). EDIT: '_' shows up once per few min :)
I think it is worth to mention that the usart commends might be slowing down the work (however its easiest to share with you way of debugging in my opinion, tell me if I am wrong) as in the original balacning robot program the interrupt on timer occurs with this last frequency.
here is this code:
TIMSK |= (1 << OCIE2); //Output Compare Interrupt Enable 2
TCCR2 |= (1 << WGM21) | (1 << CS22)|(1 <<CS21); // Clear Timer on Compare 64
OCR2 = 1; //Output Compare Register 2 (8 bit -> up to 256)
sei(); //Set Enable for Interrupt
[...]
ISR(TIMER2_COMP_vect){
extraTime++;
if (extraTime>=halfPeriod)
{
PORTB ^= (1 << PORTB0)|(1<<PORTB2); //connected to step inputs of driver
extraTime=0;
}
}Only thing that comes to my mind that could perhaps solve this problem is adding external oscilator and increasing F_CPU to 16Mhz for example.
the question is, if it is possible to do it on this frequency or perhaps there is a better way to control two motors. I am quite new to AVR enviroment and still learning a lot so I can be doing it completly wrong way and in this case please correct me.
Chris
