suk_
Published © LGPL

Makeshift Gimbal

Simple DIY two-axis gimbal using nothing but two servo motors, an mpu6050 gyro and accelerometer sensor, and obviously an Arduino (Uno).

IntermediateShowcase (no instructions)1,786
Makeshift Gimbal

Things used in this project

Story

Read more

Schematics

Gimbal Project Report

This is a report of the drone and my project that I had to submit at the end of the workshop. You can scroll down to the part where I explain the gimbal stuff and ignore the codes I have added (they definitely don't work).

Code

Interfacing servo motors with MPU6050

Arduino
#include <Wire.h>
#include <Servo.h>

int servoPin1 = 4;
int servoPin2 = 7;
Servo Servo1;
Servo Servo2;

//Gyro Variables
float elapsedTime, time, timePrev;        //Variables for time control
int gyro_error=0;                         //We use this variable to only calculate once the gyro data error
float Gyr_rawX, Gyr_rawY, Gyr_rawZ;     //Here we store the raw data read 
float Gyro_angle_x, Gyro_angle_y;         //Here we store the angle value obtained with Gyro data
float Gyro_raw_error_x, Gyro_raw_error_y; //Here we store the initial gyro data error

//Acc Variables
int acc_error=0;                         //We use this variable to only calculate once the Acc data error
float rad_to_deg = 180/3.141592654;      //This value is for pasing from radians to degrees values
float Acc_rawX, Acc_rawY, Acc_rawZ;    //Here we store the raw data read 
float Acc_angle_x, Acc_angle_y;          //Here we store the angle value obtained with Acc data
float Acc_angle_error_x, Acc_angle_error_y; //Here we store the initial Acc data error

float Total_angle_x, Total_angle_y;



void setup() {

  Servo1.attach(servoPin1);
  Servo2.attach(servoPin2);
  Servo1.write(90); 
  Servo2.write(90);
  
  Wire.begin();                           
  
  Wire.beginTransmission(0x68);                        
  Wire.write(0x6B);                       
  Wire.write(0x00);
  Wire.endTransmission(true);             
  //Gyro config
  Wire.beginTransmission(0x68);           
  Wire.write(0x1B);                       
  Wire.write(0x10);                      
  Wire.endTransmission(true);             
  //Acc config
  Wire.beginTransmission(0x68);           
  Wire.write(0x1C);                       
  Wire.write(0x10);                       
  Wire.endTransmission(true); 

  Serial.begin(9600);                     
  time = millis();                        



  if(acc_error==0)
  {
    for(int a=0; a<200; a++)
    {
      Wire.beginTransmission(0x68);
      Wire.write(0x3B);                       
      Wire.endTransmission(false);
      Wire.requestFrom(0x68,6,true);         
      
      Acc_rawX=(Wire.read()<<8|Wire.read())/4096.0 ; 
      Acc_rawY=(Wire.read()<<8|Wire.read())/4096.0 ;
      Acc_rawZ=(Wire.read()<<8|Wire.read())/4096.0 ;

      
      /*---X---*/
      Acc_angle_error_x = Acc_angle_error_x + ((atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) + pow((Acc_rawZ),2)))*rad_to_deg));
      /*---Y---*/
      Acc_angle_error_y = Acc_angle_error_y + ((atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) + pow((Acc_rawZ),2)))*rad_to_deg)); 
      
      if(a==199)
      {
        Acc_angle_error_x = Acc_angle_error_x/200;
        Acc_angle_error_y = Acc_angle_error_y/200;
        acc_error=1;
      }
    }
  }//end of acc error calculation   



  if(gyro_error==0)
  {
    for(int i=0; i<200; i++)
    {
      Wire.beginTransmission(0x68);             
      Wire.write(0x43);                        
      Wire.endTransmission(false);
      Wire.requestFrom(0x68,4,true);            
         
      Gyr_rawX=Wire.read()<<8|Wire.read();     
      Gyr_rawY=Wire.read()<<8|Wire.read();
   
      /*---X---*/
      Gyro_raw_error_x = Gyro_raw_error_x + (Gyr_rawX/32.8); 
      /*---Y---*/

      
      
      Gyro_raw_error_y = Gyro_raw_error_y + (Gyr_rawY/32.8);
      
      \
      if(i==199)
      {
        Gyro_raw_error_x = Gyro_raw_error_x/200;
        Gyro_raw_error_y = Gyro_raw_error_y/200;
        gyro_error=1;
      }
    }
  }//end of gyro error calculation   
}//end of setup void


void loop() {
  timePrev = time;                        
  time = millis();                        
  elapsedTime = (time - timePrev) / 1000; 
  //////////////////////////////////////Gyro read/////////////////////////////////////

    Wire.beginTransmission(0x68);             
    Wire.write(0x43);                       
    Wire.endTransmission(false);
    Wire.requestFrom(0x68,4,true);           
        
    Gyr_rawX=Wire.read()<<8|Wire.read();     
    Gyr_rawY=Wire.read()<<8|Wire.read();
    
    /*---X---*/
    Gyr_rawX = (Gyr_rawX/32.8) - Gyro_raw_error_x; // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
    /*---Y---*/
    Gyr_rawY = (Gyr_rawY/32.8) - Gyro_raw_error_y;  //0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
    
    
    /*---X---*/
    Gyro_angle_x = Gyr_rawX*elapsedTime;
    /*---X---*/
    Gyro_angle_y = Gyr_rawY*elapsedTime;


    
  
  //////////////////////////////////////Acc read/////////////////////////////////////

  Wire.beginTransmission(0x68);     
  Wire.write(0x3B);                 
  Wire.endTransmission(false);      
  Wire.requestFrom(0x68,6,true);    
      
  Acc_rawX=(Wire.read()<<8|Wire.read())/4096.0 ; 
  Acc_rawY=(Wire.read()<<8|Wire.read())/4096.0 ;
  Acc_rawZ=(Wire.read()<<8|Wire.read())/4096.0 ; 
 
 /*---X---*/
 Acc_angle_x = (atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) + pow((Acc_rawZ),2)))*rad_to_deg) - Acc_angle_error_x;
 /*---Y---*/
 Acc_angle_y = (atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) + pow((Acc_rawZ),2)))*rad_to_deg) - Acc_angle_error_y;    


 //////////////////////////////////////Total angle and filter/////////////////////////////////////
 /*---X axis angle---*/
 Total_angle_x = 0.98 *(Total_angle_x + Gyro_angle_x) + 0.02*Acc_angle_x;
 /*---Y axis angle---*/
 Total_angle_y = 0.98 *(Total_angle_y + Gyro_angle_y) + 0.02*Acc_angle_y;
   

 
 
 
 Serial.print("Xº: ");
 Serial.print(Total_angle_x);
 Serial.print("   ");
 Serial.print("Yº: ");
 Serial.print(Total_angle_y);
 Serial.println(" ");

  Servo1.write(Total_angle_x + 45); 
  Servo2.write(Total_angle_y + 90);
 
}

Credits

suk_

suk_

0 projects • 4 followers

Comments