Makeshift Gimbal

1#include <Wire.h>
2#include <Servo.h>
3
4int servoPin1 = 4;
5int  servoPin2 = 7;
6Servo Servo1;
7Servo Servo2;
8
9//Gyro Variables
10float  elapsedTime, time, timePrev;        //Variables for time control
11int gyro_error=0;                         //We use this variable to only calculate once the gyro  data error
12float Gyr_rawX, Gyr_rawY, Gyr_rawZ;     //Here we store the raw data  read 
13float Gyro_angle_x, Gyro_angle_y;         //Here we store the angle value  obtained with Gyro data
14float Gyro_raw_error_x, Gyro_raw_error_y; //Here we store  the initial gyro data error
15
16//Acc Variables
17int acc_error=0;                         //We  use this variable to only calculate once the Acc data error
18float rad_to_deg  = 180/3.141592654;      //This value is for pasing from radians to degrees values
19float  Acc_rawX, Acc_rawY, Acc_rawZ;    //Here we store the raw data read 
20float Acc_angle_x,  Acc_angle_y;          //Here we store the angle value obtained with Acc data
21float  Acc_angle_error_x, Acc_angle_error_y; //Here we store the initial Acc data error
22
23float  Total_angle_x, Total_angle_y;
24
25
26
27void setup() {
28
29  Servo1.attach(servoPin1);
30  Servo2.attach(servoPin2);
31  Servo1.write(90); 
32  Servo2.write(90);
33  
34  Wire.begin();                           
35  
36  Wire.beginTransmission(0x68);                        
37  Wire.write(0x6B);                       
38  Wire.write(0x00);
39  Wire.endTransmission(true);             
40  //Gyro config
41  Wire.beginTransmission(0x68);           
42  Wire.write(0x1B);                       
43  Wire.write(0x10);                      
44  Wire.endTransmission(true);             
45  //Acc  config
46  Wire.beginTransmission(0x68);           
47  Wire.write(0x1C);                       
48  Wire.write(0x10);                       
49  Wire.endTransmission(true); 
50
51  Serial.begin(9600);                     
52  time = millis();                        
53
54
55
56  if(acc_error==0)
57  {
58    for(int a=0; a<200; a++)
59    {
60      Wire.beginTransmission(0x68);
61      Wire.write(0x3B);                       
62      Wire.endTransmission(false);
63      Wire.requestFrom(0x68,6,true);         
64      
65      Acc_rawX=(Wire.read()<<8|Wire.read())/4096.0  ; 
66      Acc_rawY=(Wire.read()<<8|Wire.read())/4096.0 ;
67      Acc_rawZ=(Wire.read()<<8|Wire.read())/4096.0  ;
68
69      
70      /*---X---*/
71      Acc_angle_error_x = Acc_angle_error_x  + ((atan((Acc_rawY)/sqrt(pow((Acc_rawX),2) + pow((Acc_rawZ),2)))*rad_to_deg));
72      /*---Y---*/
73      Acc_angle_error_y = Acc_angle_error_y + ((atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2)  + pow((Acc_rawZ),2)))*rad_to_deg)); 
74      
75      if(a==199)
76      {
77        Acc_angle_error_x = Acc_angle_error_x/200;
78        Acc_angle_error_y  = Acc_angle_error_y/200;
79        acc_error=1;
80      }
81    }
82  }//end  of acc error calculation   
83
84
85
86  if(gyro_error==0)
87  {
88    for(int  i=0; i<200; i++)
89    {
90      Wire.beginTransmission(0x68);             
91      Wire.write(0x43);                        
92      Wire.endTransmission(false);
93      Wire.requestFrom(0x68,4,true);            
94         
95      Gyr_rawX=Wire.read()<<8|Wire.read();     
96      Gyr_rawY=Wire.read()<<8|Wire.read();
97   
98      /*---X---*/
99      Gyro_raw_error_x = Gyro_raw_error_x + (Gyr_rawX/32.8); 
100      /*---Y---*/
101
102      
103      
104      Gyro_raw_error_y = Gyro_raw_error_y + (Gyr_rawY/32.8);
105      
106      \\
107      if(i==199)
108      {
109        Gyro_raw_error_x =  Gyro_raw_error_x/200;
110        Gyro_raw_error_y = Gyro_raw_error_y/200;
111        gyro_error=1;
112      }
113    }
114  }//end of gyro error calculation   
115}//end of setup void
116
117
118void  loop() {
119  timePrev = time;                        
120  time = millis();                        
121  elapsedTime = (time - timePrev) / 1000; 
122  //////////////////////////////////////Gyro  read/////////////////////////////////////
123
124    Wire.beginTransmission(0x68);             
125    Wire.write(0x43);                       
126    Wire.endTransmission(false);
127    Wire.requestFrom(0x68,4,true);           
128        
129    Gyr_rawX=Wire.read()<<8|Wire.read();     
130    Gyr_rawY=Wire.read()<<8|Wire.read();
131    
132    /*---X---*/
133    Gyr_rawX = (Gyr_rawX/32.8) - Gyro_raw_error_x; // 0x43 (GYRO_XOUT_H) & 0x44  (GYRO_XOUT_L)
134    /*---Y---*/
135    Gyr_rawY = (Gyr_rawY/32.8) - Gyro_raw_error_y;  //0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
136    
137    
138    /*---X---*/
139    Gyro_angle_x = Gyr_rawX*elapsedTime;
140    /*---X---*/
141    Gyro_angle_y  = Gyr_rawY*elapsedTime;
142
143
144    
145  
146  //////////////////////////////////////Acc  read/////////////////////////////////////
147
148  Wire.beginTransmission(0x68);     
149  Wire.write(0x3B);                 
150  Wire.endTransmission(false);      
151  Wire.requestFrom(0x68,6,true);    
152      
153  Acc_rawX=(Wire.read()<<8|Wire.read())/4096.0  ; 
154  Acc_rawY=(Wire.read()<<8|Wire.read())/4096.0 ;
155  Acc_rawZ=(Wire.read()<<8|Wire.read())/4096.0  ; 
156 
157 /*---X---*/
158 Acc_angle_x = (atan((Acc_rawY)/sqrt(pow((Acc_rawX),2)  + pow((Acc_rawZ),2)))*rad_to_deg) - Acc_angle_error_x;
159 /*---Y---*/
160 Acc_angle_y  = (atan(-1*(Acc_rawX)/sqrt(pow((Acc_rawY),2) + pow((Acc_rawZ),2)))*rad_to_deg) -  Acc_angle_error_y;    
161
162
163 //////////////////////////////////////Total angle  and filter/////////////////////////////////////
164 /*---X axis angle---*/
165 Total_angle_x  = 0.98 *(Total_angle_x + Gyro_angle_x) + 0.02*Acc_angle_x;
166 /*---Y axis angle---*/
167  Total_angle_y = 0.98 *(Total_angle_y + Gyro_angle_y) + 0.02*Acc_angle_y;
168   
169
170  
171 
172 
173 Serial.print("Xº: ");
174 Serial.print(Total_angle_x);
175 Serial.print("   ");
176 Serial.print("Yº: ");
177 Serial.print(Total_angle_y);
178 Serial.println("  ");
179
180  Servo1.write(Total_angle_x + 45); 
181  Servo2.write(Total_angle_y  + 90);
182 
183}