Hi all! I am making a robot leg with stepper motor controlled by MPU6050. I plan to rotate the motor clockwise or anti-clockwise if the angle is within the range set in the program, e.g. motor rotates clockwise if the angle is within 30 to 50 degree. But the angle is not updating after the motor starts running. It looks like the program is looping inside the for loop and not breaking out. Is it related to the timer of MPU6050? and is there any ways to solve this problem? Thank you!
Here is our code:
#include <Wire.h>
#include <Stepper.h>
const int stepsPerRevolution = 100;
Stepper myStepper(stepsPerRevolution, 8, 9, 10, 11);
//Pin number where the servo motor is connected. (Digital PWM 3)
#define Stepper_Pin 8
#define Stepper_Pin 9
#define Stepper_Pin 10
#define Stepper_Pin 11
//Declaring some global variables
int gyro_x, gyro_y, gyro_z;
long gyro_x_cal, gyro_y_cal, gyro_z_cal;
boolean set_gyro_angles;
long acc_x, acc_y, acc_z, acc_total_vector;
float angle_roll_acc, angle_pitch_acc;
float angle_pitch, angle_roll;
int angle_pitch_buffer, angle_roll_buffer;
static float angle_pitch_output, angle_roll_output;
long loop_timer;
int temp;
float minangle1;
float minangle2;
float maxangle1;
float maxangle2;
int count;
#define minangle1 -40
#define minangle2 -30
#define maxangle1 30
#define maxangle2 35
void setup() {
myStepper.setSpeed(60);
Wire.begin();
setup_mpu_6050_registers();
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){
read_mpu_6050_data();
gyro_x_cal += gyro_x;
gyro_y_cal += gyro_y;
gyro_z_cal += gyro_z;
delay(3);
}
// divide by 1000 to get avarage offset
gyro_x_cal /= 1000;
gyro_y_cal /= 1000;
gyro_z_cal /= 1000;
Serial.begin(115200);
loop_timer = micros();
}
void run_gyro(){
read_mpu_6050_data();
//Subtract the offset values from the raw gyro values
gyro_x -= gyro_x_cal;
gyro_y -= gyro_y_cal;
gyro_z -= gyro_z_cal;
//Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
angle_pitch += gyro_x * 0.0000611;
angle_roll += gyro_y * 0.0000611;
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
angle_pitch += angle_roll * sin(gyro_z * 0.000001066);
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066);
//Accelerometer angle calculations
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296;
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296;
angle_pitch_acc -= 0.0;
angle_roll_acc -= 0.0;
if(set_gyro_angles){
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004;
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004;
}
else{
angle_pitch = angle_pitch_acc;
angle_roll = angle_roll_acc;
set_gyro_angles = true;
}
//To dampen the pitch and roll angles a complementary filter is used
angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1;
angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1;
Serial.print(" | Angle = "); Serial.println((angle_roll_output));
while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
loop_timer = micros();//Reset the loop timer
}
void setup_mpu_6050_registers(){
//Activate the MPU-6050
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x6B); //Send the requested starting register
Wire.write(0x00); //Set the requested starting register
Wire.endTransmission();
//Configure the accelerometer (+/-8g)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1C); //Send the requested starting register
Wire.write(0x10); //Set the requested starting register
Wire.endTransmission();
//Configure the gyro (500dps full scale)
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x1B); //Send the requested starting register
Wire.write(0x08); //Set the requested starting register
Wire.endTransmission();
}
void read_mpu_6050_data(){ //Subroutine for reading the raw gyro and accelerometer data
Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
Wire.write(0x3B); //Send the requested starting register
Wire.endTransmission(); //End the transmission
Wire.requestFrom(0x68,14); //Request 14 bytes from the MPU-6050
while(Wire.available() < 14); //Wait until all the bytes are received
acc_x = Wire.read()<<8|Wire.read();
acc_y = Wire.read()<<8|Wire.read();
acc_z = Wire.read()<<8|Wire.read();
temp = Wire.read()<<8|Wire.read();
gyro_x = Wire.read()<<8|Wire.read();
gyro_y = Wire.read()<<8|Wire.read();
gyro_z = Wire.read()<<8|Wire.read();
}
void raiseleg(){
int count;
for (count = 0; count <=2; count++)
{
Serial.println("clockwise");
myStepper.step(stepsPerRevolution);
delay(20);
}
}
void lowerleg(){
for (int count = 0; count <= 2; count++)
{
Serial.println("anticlockwise");
myStepper.step(stepsPerRevolution);
delay(20);
}
void sittostand()
{
for (int count = 0; count <= 2; count++) //modification no of count
{
Serial.println("sittostand");
myStepper.step(stepsPerRevolution);
delay(50);
}
}
void standtosit()
{
for (int count = 0; count <= 2; count++) //modification
{
Serial.println("standtosit");
myStepper.step(-stepsPerRevolution);
delay(10);
}
}
void loop()
{ run_gyro();
if ((angle_roll_output) > -40 && (angle_roll_output) < -30))
{sittostand();}
if(angle_roll_output > maxangle1 && angle_roll_output < maxangle2 )
{raiseleg();
lowerleg();}
if ((angle_roll_output) > -20 && (angle_roll_output) < -10))
{standtosit();}
}
