Hi, this is my first post, so please be understanding if I write silly things ;). I am also pretty new to Arduino. As in topic, I use Nema17 stepper motors, stepper motor driver A4988, MPU6050 and Arduino Nano.
I'm doing kind of balancing robot for my degree project. The snag is that I'm learning how to do it from scratch step by step. I faced a problem learning how the interrupts works - specifically I want the 8-bit Timer2 to call the ISR in compare mode which will set the state on the STEP pin on both stepper motors and drive them in the direction depending on the tilt angle from the gyroscope in the IMU MPU6050.
Here's the code:
#include "Wire.h"
#include <MPU6050_light.h>
MPU6050 mpu(Wire);
const int LEFT_DIR_PIN = 8;
const int RIGHT_DIR_PIN = 7;
const int LEFT_STEP_PIN = 6;
const int RIGHT_STEP_PIN = 5;
const int BOTH_ENABLE_PIN = 4; // soldered together to save pins ;)
int motor_direction = 0; // 1 forward, -1 backward, 0 stop
// --------------------------------- ISR for timer 2 - compare mode ----------------
ISR(TIMER2_COMPA_vect)
{
if(motor_direction == 0){
return;
}
digitalWrite(RIGHT_STEP_PIN, HIGH);
digitalWrite(LEFT_STEP_PIN, HIGH);
delayMicroseconds(1000);
digitalWrite(RIGHT_STEP_PIN, LOW);
digitalWrite(LEFT_STEP_PIN, LOW);
}
// --------------------------------- ISR for timer 2 - compare mode ----------------
void setup() {
// --------------------------------- MPU6050 ---------------------------------------
Serial.begin(9600);
Wire.begin();
byte status = mpu.begin();
Serial.println("MPU6050 status: ");
Serial.println(status);
while(status!=0){ } // stop everything if can't connect to MPU6050
Serial.println("Calculating offsets, do not move MPU6050");
delay(1000);
mpu.calcOffsets(); // offsets for gyroscope and accelerometer
Serial.println("Done!\n");
// --------------------------------- MPU6050 ---------------------------------------
pinMode(LEFT_STEP_PIN, OUTPUT);
pinMode(LEFT_DIR_PIN, OUTPUT);
pinMode(RIGHT_STEP_PIN, OUTPUT);
pinMode(RIGHT_DIR_PIN, OUTPUT);
pinMode(BOTH_ENABLE_PIN, OUTPUT);
// Timer 2
TCCR2A = 0; //Make sure that the TCCR2A register is set to zero
TCCR2B = 0; //Make sure that the TCCR2A register is set to zero
TIMSK2 |= (1 << OCIE2A); //Set the interupt enable bit OCIE2A in the TIMSK2 register
TCCR2B |= (1 << CS21); //Set the CS21 bit in the TCCRB register to set the prescaler to 8
OCR2A = 39; //The compare register is set to 39 => 20us / (1s / (16.000.000MHz / 8)) - 1
TCCR2A |= (1 << WGM21); //Set counter 2 to CTC (clear timer on compare) mode
// TCNT2 = 0;
}
void loop() {
mpu.update();
if(mpu.getAngleX() >= 2){
digitalWrite(BOTH_ENABLE_PIN, LOW); // enable stepper motors
digitalWrite(RIGHT_DIR_PIN, LOW);
digitalWrite(LEFT_DIR_PIN, HIGH);
motor_direction = -1; // backward
mpu.update();
}
else if(mpu.getAngleX()<2 && mpu.getAngleX()>-2){
digitalWrite(BOTH_ENABLE_PIN, HIGH); // disable stepper motors
motor_direction = 0; // stop
mpu.update();
}
else{
digitalWrite(BOTH_ENABLE_PIN, LOW); // enable stepper motors
digitalWrite(RIGHT_DIR_PIN, HIGH);
digitalWrite(LEFT_DIR_PIN, LOW);
motor_direction = 1; // forward
mpu.update();
}
}
When the MPU6050 starts up, it calibrates itself and sets the "zero" position for the robot's balance. Then when I tilt the robot to one side, the motors respond correctly, but they can no longer come out of this operation. Even when I tilt the robot to the other side, the motors still spin to the first tilted side.
Would anyone have any idea what was done wrong here and how to possibly improve the performance/smoothness for the future whole balancing robot program? Thank you for any help.
.