Go Down

Topic: How to control stepper motor with MPU6050 (Read 111 times) previous topic - next topic

liuliu55

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:
Code: [Select]

#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();}
 }
 

Power_Broker

It looks like the program is looping inside the for loop and not breaking out.
That tends to be what for loops do...


You should be using a PID controller in your code instead of whatever you're trying to do right now.
"The desire that guides me in all I do is the desire to harness the forces of nature to the service of mankind."
   - Nikola Tesla

MarkT

Your code isn't going to work well, rotation just doesn't work like that, you cannot handle pitch and roll
independently, that gives wrong answers.

What you need to do is work in 3D (normally using quarternions) and convert to Euler angles at the end.  The quarternion method of calculating rotations works correctly.
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

Idahowalker

Hi all! I am making a robot leg with stepper motor controlled by MPU6050. I plan to rotate the motor clockwise or Thank you!

https://os.mbed.com/users/onehorse/

Go Up