1 | #include <Wire.h>
|
2 |
|
3 | int gyro_x, gyro_y, gyro_z;
|
4 | int angle_pitch_buffer, angle_roll_buffer;
|
5 | int temp;
|
6 | int count = 0;
|
7 |
|
8 | long gyro_x_cal, gyro_y_cal, gyro_z_cal;
|
9 | long acc_x, acc_y, acc_z, acc_total_vector;
|
10 | long loop_timer;
|
11 | long acc_x_average, acc_y_average, acc_z_average;
|
12 |
|
13 | long gyro_x_offset = 0;
|
14 | long gyro_y_offset = 0;
|
15 | long gyro_z_offset = 0;
|
16 |
|
17 | long acc_x_offset = 0; // 10
|
18 | long acc_y_offset = 0; // 180
|
19 | long acc_z_offset = 0; // -225
|
20 |
|
21 | boolean set_gyro_angles;
|
22 | boolean toggle;
|
23 |
|
24 | float angle_roll_acc, angle_pitch_acc;
|
25 | float angle_pitch, angle_roll;
|
26 | float angle_pitch_output, angle_roll_output;
|
27 |
|
28 | void setup()
|
29 | {
|
30 | Wire.begin(); //Start I2C as master
|
31 |
|
32 | Serial.begin(115200);
|
33 |
|
34 | setup_mpu_6050_registers(); //Setup the registers of the MPU-6050
|
35 |
|
36 | Serial.println("Calibrating gyro");
|
37 |
|
38 | for(int cal_int = 0; cal_int < 500 ; cal_int ++)
|
39 | { //Read the raw acc and gyro data from the MPU-6050 for 500 times
|
40 | read_mpu_6050_data();
|
41 | gyro_x_cal += gyro_x; //Add the gyro x offset to the gyro_x_cal variable
|
42 | gyro_y_cal += gyro_y; //Add the gyro y offset to the gyro_y_cal variable
|
43 | gyro_z_cal += gyro_z; //Add the gyro z offset to the gyro_z_cal variable
|
44 | delay(3); //Delay 3us to have 250Hz for-loop
|
45 | }
|
46 |
|
47 | pinMode(8, OUTPUT);
|
48 | pinMode(9, OUTPUT);
|
49 | pinMode(10, OUTPUT);
|
50 |
|
51 | // divide by 500 to get avarage offset
|
52 | gyro_x_cal /= 500;
|
53 | gyro_y_cal /= 500;
|
54 | gyro_z_cal /= 500;
|
55 |
|
56 | Serial.begin(115200);
|
57 |
|
58 | loop_timer = micros(); //Reset the loop timer
|
59 | }
|
60 |
|
61 | void loop(){
|
62 |
|
63 | read_mpu_6050_data();
|
64 |
|
65 | //Subtract the offset values from the raw gyro values
|
66 | gyro_x -= gyro_x_cal;
|
67 | gyro_y -= gyro_y_cal;
|
68 | gyro_z -= gyro_z_cal;
|
69 | acc_x += acc_x_offset;
|
70 | acc_y += acc_y_offset;
|
71 | acc_z += acc_z_offset;
|
72 |
|
73 | //Gyro angle calculations . Note 0.0000611 = 1 / (250Hz x 65.5)
|
74 | angle_pitch += gyro_x * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable
|
75 | angle_roll += gyro_y * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable
|
76 |
|
77 | //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians
|
78 | angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
|
79 | angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
|
80 |
|
81 | //Accelerometer angle calculations
|
82 | acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector
|
83 |
|
84 | //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
|
85 | angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle
|
86 | angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
|
87 |
|
88 | angle_pitch_acc -= -2.04; //Accelerometer calibration value for pitch
|
89 | angle_roll_acc -= 0.38; //Accelerometer calibration value for roll
|
90 |
|
91 | if(set_gyro_angles)
|
92 | { //If the IMU is already started
|
93 | angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
|
94 | angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Correct the drift of the gyro roll angle with the accelerometer roll angle
|
95 | }
|
96 |
|
97 | else
|
98 | { //At first start
|
99 | angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
|
100 | angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
|
101 | set_gyro_angles = true; //Set the IMU started flag
|
102 | }
|
103 |
|
104 | //To dampen the pitch and roll angles a complementary filter is used
|
105 | angle_pitch_output = angle_pitch_output * 0.9 + angle_pitch * 0.1; //Take 90% of the output pitch value and add 10% of the raw pitch value
|
106 | angle_roll_output = angle_roll_output * 0.9 + angle_roll * 0.1; //Take 90% of the output roll value and add 10% of the raw roll value
|
107 |
|
108 | acc_x_average += acc_x;
|
109 | acc_y_average += acc_y;
|
110 | acc_z_average += acc_z;
|
111 | count++;
|
112 |
|
113 | if(count == 100)
|
114 | {
|
115 | Serial.print(acc_x_average/100);
|
116 | Serial.print("\t");
|
117 | Serial.print(acc_y_average/100);
|
118 | Serial.print("\t");
|
119 | Serial.print(acc_z_average/100);
|
120 | Serial.print("\t\t");
|
121 | Serial.print(angle_pitch_output);
|
122 | Serial.print("\t");
|
123 | Serial.println(angle_roll_output);
|
124 | count = 0;
|
125 | acc_x_average = 0;
|
126 | acc_y_average = 0;
|
127 | acc_z_average = 0;
|
128 | }
|
129 |
|
130 |
|
131 | // half frequency output
|
132 | toggle = !toggle; if(!toggle) { PORTB |= (1 << PB2); } else { PORTB &= ~(1 << PB2); }
|
133 |
|
134 | if(angle_pitch_output <= 90.1 && angle_pitch_output >= 89.9) { digitalWrite(8, HIGH); } else { digitalWrite(8, LOW); }
|
135 | if(angle_roll_output <= 90.1 && angle_roll_output >= 89.9) { digitalWrite(9, HIGH); } else { digitalWrite(9, LOW); }
|
136 |
|
137 | while(micros() - loop_timer < 4000); //Wait until the loop_timer reaches 4000us (250Hz) before starting the next loop
|
138 |
|
139 | loop_timer = micros();//Reset the loop timer
|
140 | }
|
141 |
|
142 |
|
143 |
|
144 |
|
145 | void setup_mpu_6050_registers()
|
146 | {
|
147 | //Activate the MPU-6050
|
148 | Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
|
149 | Wire.write(0x6B); //Send the requested starting register
|
150 | Wire.write(0x00); //Set the requested starting register
|
151 | Wire.endTransmission();
|
152 | //Configure the accelerometer (+/-8g)
|
153 | Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
|
154 | Wire.write(0x1C); //Send the requested starting register
|
155 | Wire.write(0x10); //Set the requested starting register
|
156 | Wire.endTransmission();
|
157 | //Configure the gyro (500dps full scale)
|
158 | Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
|
159 | Wire.write(0x1B); //Send the requested starting register
|
160 | Wire.write(0x08); //Set the requested starting register
|
161 | Wire.endTransmission();
|
162 | }
|
163 |
|
164 | void read_mpu_6050_data()
|
165 | { //Subroutine for reading the raw gyro and accelerometer data
|
166 | Wire.beginTransmission(0x68); //Start communicating with the MPU-6050
|
167 | Wire.write(0x3B); //Send the requested starting register
|
168 | Wire.endTransmission(); //End the transmission
|
169 | Wire.requestFrom(0x68,14); //Request 14 bytes from the MPU-6050
|
170 | while(Wire.available() < 14); //Wait until all the bytes are received
|
171 | acc_x = Wire.read()<<8|Wire.read();
|
172 | acc_y = Wire.read()<<8|Wire.read();
|
173 | acc_z = Wire.read()<<8|Wire.read();
|
174 | temp = Wire.read()<<8|Wire.read();
|
175 | gyro_x = Wire.read()<<8|Wire.read();
|
176 | gyro_y = Wire.read()<<8|Wire.read();
|
177 | gyro_z = Wire.read()<<8|Wire.read();
|
178 | }
|
Thread beobachten
Seitenaufteilung abschalten