1 | // 2 servo planar stabilization system
|
2 | // wp
|
3 | // Jan 2016
|
4 | //
|
5 | // Based on Jeff Rowber's work found at
|
6 | // https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/MPU6050.cpp
|
7 | //
|
8 | // Use at your own risk.
|
9 | //
|
10 | // This code is placed under the MIT License (MIT)
|
11 | //
|
12 | // Copyright (c) 2016 woojay poynter
|
13 |
|
14 | //Permission is hereby granted, free of charge, to any person obtaining a copy
|
15 | //of this software and associated documentation files (the "Software"), to deal
|
16 | //in the Software without restriction, including without limitation the rights
|
17 | //to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
18 | //copies of the Software, and to permit persons to whom the Software is
|
19 | //furnished to do so, subject to the following conditions:
|
20 |
|
21 | //The above copyright notice and this permission notice shall be included in all
|
22 | //copies or substantial portions of the Software.
|
23 |
|
24 | //THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
25 | //IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
26 | //FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
27 | //AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
28 | //LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
29 | //OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
30 | //SOFTWARE.
|
31 | //
|
32 |
|
33 | // Servo Connection
|
34 | // BROWN - gnd
|
35 | // red - 5v
|
36 | // yellow - d10 (pwm on Sero 1)
|
37 | // - d11 (servo 2)
|
38 |
|
39 | // MPU Connection
|
40 | //
|
41 | // VCC - 5v
|
42 | // GND - GND
|
43 | // SCL - A5 (w/ 10k PuR)
|
44 | // SDA - A4 (w/ 10k PuR)
|
45 | // INT - D2 (not used)
|
46 |
|
47 | #include <Servo.h>
|
48 | #include "I2Cdev.h"
|
49 | #include "MPU6050_6Axis_MotionApps20.h"
|
50 | //#include "MPU6050.h" // not necessary if using MotionApps include file
|
51 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
52 | #include "Wire.h"
|
53 | #endif
|
54 |
|
55 | #define LED_PIN 13
|
56 | bool blinkState = true;
|
57 |
|
58 | Servo Servo1; // First Servo off the chassis
|
59 | Servo Servo2; // Second Servo off the chassis
|
60 |
|
61 | int Servo1Pos = 0;
|
62 | int Servo2Pos = 0;
|
63 |
|
64 | float mpuPitch = 0;
|
65 | float mpuRoll = 0;
|
66 | float mpuYaw = 0;
|
67 |
|
68 |
|
69 | // define MPU instance
|
70 | MPU6050 mpu; // class default I2C address is 0x68; specific I2C addresses may be passed as a parameter here
|
71 |
|
72 | // MPU control/status vars
|
73 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
|
74 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
|
75 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
|
76 | uint16_t fifoCount; // count of all bytes currently in FIFO
|
77 | uint8_t fifoBuffer[64]; // FIFO storage buffer
|
78 |
|
79 | // orientation/motion vars
|
80 | Quaternion q; // [w, x, y, z] quaternion container
|
81 | VectorInt16 aa; // [x, y, z] accel sensor measurements
|
82 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
|
83 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
|
84 | VectorFloat gravity; // [x, y, z] gravity vector
|
85 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
86 |
|
87 | // relative ypr[x] usage based on sensor orientation when mounted, e.g. ypr[PITCH]
|
88 | #define PITCH 1 // defines the position within ypr[x] variable for PITCH; may vary due to sensor orientation when mounted
|
89 | #define ROLL 2 // defines the position within ypr[x] variable for ROLL; may vary due to sensor orientation when mounted
|
90 | #define YAW 0 // defines the position within ypr[x] variable for YAW; may vary due to sensor orientation when mounted
|
91 |
|
92 | // ================================================================
|
93 | // === INITIAL SETUP ===
|
94 | // ================================================================
|
95 |
|
96 | void setup()
|
97 | {
|
98 |
|
99 | Servo1.attach(10); // attaches the servo on D11 to the servo object
|
100 | Servo2.attach(11); // Second servo on D11
|
101 | delay(50);
|
102 | Servo1.write(0); // These are command checks to see if the servos work and
|
103 | Servo2.write(60); // to help w/ the initial installation.
|
104 | delay(500); // Make sure these movements are clear from the rest of the chassis.
|
105 | Servo1.write(180);
|
106 | Servo2.write(120);
|
107 | delay(500);
|
108 | Servo1.write(0);
|
109 | Servo2.write(90);
|
110 | delay(500);
|
111 |
|
112 | // join I2C bus (I2Cdev library doesn't do this automatically)
|
113 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
114 | Wire.begin();
|
115 | TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
|
116 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
117 | Fastwire::setup(400, true);
|
118 | #endif
|
119 |
|
120 | Serial.begin(115200);
|
121 | while (!Serial); // wait for Leonardo enumeration, others continue immediately
|
122 |
|
123 | // initialize device
|
124 | Serial.println(F("Initializing I2C devices..."));
|
125 | mpu.initialize();
|
126 |
|
127 | // verify connection
|
128 | Serial.println(F("Testing device connections..."));
|
129 | Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
|
130 |
|
131 | // load and configure the DMP
|
132 | Serial.println(F("Initializing DMP"));
|
133 | devStatus = mpu.dmpInitialize();
|
134 |
|
135 |
|
136 | // INPUT CALIBRATED OFFSETS HERE; SPECIFIC FOR EACH UNIT AND EACH MOUNTING CONFIGURATION!!!!
|
137 |
|
138 | mpu.setXGyroOffset(153);
|
139 | mpu.setYGyroOffset(-70);
|
140 | mpu.setZGyroOffset(5);
|
141 | mpu.setXAccelOffset(-2361);
|
142 | mpu.setYAccelOffset(-1722);
|
143 | mpu.setZAccelOffset(1225 );
|
144 |
|
145 | // make sure it worked (returns 0 if so)
|
146 | if (devStatus == 0)
|
147 | {
|
148 | // turn on the DMP, now that it's ready
|
149 | Serial.println(F("Enabling DMP"));
|
150 | mpu.setDMPEnabled(true);
|
151 |
|
152 | // enable Arduino interrupt detection
|
153 | Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)"));
|
154 | mpuIntStatus = mpu.getIntStatus();
|
155 |
|
156 | // get expected DMP packet size for later comparison
|
157 | packetSize = mpu.dmpGetFIFOPacketSize();
|
158 | }
|
159 | else
|
160 | {
|
161 | // ERROR!
|
162 | // 1 = initial memory load failed, 2 = DMP configuration updates failed (if it's going to break, usually the code will be 1)
|
163 | Serial.print(F("DMP Initialization failed code = "));
|
164 | Serial.println(devStatus);
|
165 | }
|
166 |
|
167 | // configure LED for output
|
168 | pinMode(LED_PIN, OUTPUT);
|
169 |
|
170 | } // setup()
|
171 |
|
172 |
|
173 |
|
174 | // ================================================================
|
175 | // === MAIN PROGRAM LOOP ===
|
176 | // ================================================================
|
177 |
|
178 | void loop(void)
|
179 | {
|
180 | processAccelGyro();
|
181 | } // loop()
|
182 |
|
183 |
|
184 |
|
185 | // ================================================================
|
186 | // === PROCESS ACCEL/GYRO IF AVAILABLE ===
|
187 | // ================================================================
|
188 |
|
189 | void processAccelGyro()
|
190 | {
|
191 |
|
192 | // Get INT_STATUS byte
|
193 | mpuIntStatus = mpu.getIntStatus();
|
194 |
|
195 | // get current FIFO count
|
196 | fifoCount = mpu.getFIFOCount();
|
197 |
|
198 | // check for overflow (this should never happen unless our code is too inefficient)
|
199 | if ((mpuIntStatus & 0x10) || fifoCount == 1024)
|
200 | {
|
201 | // reset so we can continue cleanly
|
202 | mpu.resetFIFO();
|
203 | Serial.println(F("FIFO overflow!"));
|
204 | return;
|
205 | }
|
206 |
|
207 | if (mpuIntStatus & 0x02) // otherwise continue processing
|
208 | {
|
209 | // check for correct available data length
|
210 | if (fifoCount < packetSize)
|
211 | return; // fifoCount = mpu.getFIFOCount();
|
212 |
|
213 | // read a packet from FIFO
|
214 | mpu.getFIFOBytes(fifoBuffer, packetSize);
|
215 |
|
216 | // track FIFO count here in case there is > 1 packet available
|
217 | fifoCount -= packetSize;
|
218 |
|
219 | // flush buffer to prevent overflow
|
220 | mpu.resetFIFO();
|
221 |
|
222 | // display Euler angles in degrees
|
223 | mpu.dmpGetQuaternion(&q, fifoBuffer);
|
224 | mpu.dmpGetGravity(&gravity, &q);
|
225 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
226 | mpuPitch = ypr[PITCH] * 160 / M_PI;
|
227 | mpuRoll = ypr[ROLL] * 160 / M_PI;
|
228 | mpuYaw = ypr[YAW] * 180 / M_PI;
|
229 |
|
230 | // flush buffer to prevent overflow
|
231 | mpu.resetFIFO();
|
232 |
|
233 | // blink LED to indicate activity
|
234 | blinkState = !blinkState;
|
235 | digitalWrite(LED_PIN, blinkState);
|
236 |
|
237 | // flush buffer to prevent overflow
|
238 | mpu.resetFIFO();
|
239 |
|
240 | Servo1.write(-mpuPitch + 90);
|
241 | Servo2.write(-mpuRoll + 90);
|
242 | //delay(10);
|
243 |
|
244 | // flush buffer to prevent overflow
|
245 | mpu.resetFIFO();
|
246 |
|
247 | } // if (mpuIntStatus & 0x02)
|
248 | } // processAccelGyro()
|
Thread beobachten
Seitenaufteilung abschalten