Britney Dorval
Code for robot
Fork of
Yusheng-final_project
by 
Britney Dorval
Diff: main.cpp
- Revision:
- 2:42c4f3a7813f
- Parent:
- 1:ec0a08108442
- Child:
- 3:2f76ffbc5cef
--- a/main.cpp Sat Nov 23 16:47:59 2013 +0000
+++ b/main.cpp Sun Jan 31 14:12:13 2016 +0000
@@ -1,343 +1,17 @@
-// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
-// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
-// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//#include "MPU6050_DMP6.h"
//
-// Changelog:
-// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
-// 2012-06-20 - improved FIFO overflow handling and simplified read process
-// 2012-06-19 - completely rearranged DMP initialization code and simplification
-// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
-// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
-// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
-// - add 3D math helper file to DMP6 example sketch
-// - add Euler output and Yaw/Pitch/Roll output formats
-// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
-// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
-// 2012-05-30 - basic DMP initialization working
-
-/* ============================================
-I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-===============================================
-*/
-
-// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
-// is used in I2Cdev.h
-//#include "Wire.h"
-
-// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
-// for both classes must be in the include path of your project
-#include "I2Cdev.h"
-
-#include "MPU6050_6Axis_MotionApps20.h"
-//#include "MPU6050.h" // not necessary if using MotionApps include file
-
-// class default I2C address is 0x68
-// specific I2C addresses may be passed as a parameter here
-// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
-// AD0 high = 0x69
-
-MPU6050 mpu;
-
-/* =========================================================================
- NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
- depends on the MPU-6050's INT pin being connected to the Arduino's
- external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
- digital I/O pin 2.
- * ========================================================================= */
-
-/* =========================================================================
- NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
- when using Serial.write(buf, len). The Teapot output uses this method.
- The solution requires a modification to the Arduino USBAPI.h file, which
- is fortunately simple, but annoying. This will be fixed in the next IDE
- release. For more info, see these links:
+//int main() {
+// MPU6050DMP6::setup();
+// while(1) {
+// MPU6050DMP6::loop();
+// }
+//}
- http://arduino.cc/forum/index.php/topic,109987.0.html
- http://code.google.com/p/arduino/issues/detail?id=958
- * ========================================================================= */
-
-const float M_PI = 3.14159265;
-
-// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
-// quaternion components in a [w, x, y, z] format (not best for parsing
-// on a remote host such as Processing or something though)
-//#define OUTPUT_READABLE_QUATERNION
-
-// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
-// (in degrees) calculated from the quaternions coming from the FIFO.
-// Note that Euler angles suffer from gimbal lock (for more info, see
-// http://en.wikipedia.org/wiki/Gimbal_lock)
-//#define OUTPUT_READABLE_EULER
-
-// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
-// pitch/roll angles (in degrees) calculated from the quaternions coming
-// from the FIFO. Note this also requires gravity vector calculations.
-// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
-// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
-//#define OUTPUT_READABLE_YAWPITCHROLL
-
-// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
-// components with gravity removed. This acceleration reference frame is
-// not compensated for orientation, so +X is always +X according to the
-// sensor, just without the effects of gravity. If you want acceleration
-// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
-//#define OUTPUT_READABLE_REALACCEL
-
-// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
-// components with gravity removed and adjusted for the world frame of
-// reference (yaw is relative to initial orientation, since no magnetometer
-// is present in this case). Could be quite handy in some cases.
-//#define OUTPUT_READABLE_WORLDACCEL
-
-// uncomment "OUTPUT_TEAPOT" if you want output that matches the
-// format used for the InvenSense teapot demo
-//#define OUTPUT_TEAPOT
-
-// MPU control/status vars
-bool dmpReady = false; // set true if DMP init was successful
-uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
-uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
-uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
-uint16_t fifoCount; // count of all bytes currently in FIFO
-uint8_t fifoBuffer[64]; // FIFO storage buffer
-
-// orientation/motion vars
-Quaternion q; // [w, x, y, z] quaternion container
-VectorInt16 aa; // [x, y, z] accel sensor measurements
-VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
-VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
-VectorFloat gravity; // [x, y, z] gravity vector
-float euler[3]; // [psi, theta, phi] Euler angle container
-float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
-
-// packet structure for InvenSense teapot demo
-uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
-
-DigitalOut led1(LED1);
-InterruptIn checkpin(p29);
-Serial pc(USBTX, USBRX);
-
-// ================================================================
-// === INTERRUPT DETECTION ROUTINE ===
-// ================================================================
-
-volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
-void dmpDataReady() {
- mpuInterrupt = true;
-}
-
-void setup();
-void loop();
+#include "MPU6050_raw.h"
int main() {
- setup();
+ MPU6050raw::setup();
while(1) {
- loop();
- }
-}
-
-// ================================================================
-// === INITIAL SETUP ===
-// ================================================================
-
-void setup() {
- // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
- // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
- // the baud timing being too misaligned with processor ticks. You must use
- // 38400 or slower in these cases, or use some kind of external separate
- // crystal solution for the UART timer.
-
- // initialize device
- pc.printf("Initializing I2C devices...\r\n");
- mpu.initialize();
-
- // verify connection
- pc.printf("Testing device connections...\r\n");
- if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
- else pc.printf("MPU6050 connection failed\r\n");
-
- // wait for ready
- //Serial.println(F("\nSend any character to begin DMP programming and demo: "));
- //while (Serial.available() && Serial.read()); // empty buffer
- //while (!Serial.available()); // wait for data
- //while (Serial.available() && Serial.read()); // empty buffer again
-
- // load and configure the DMP
- pc.printf("Initializing DMP...\r\n");
- devStatus = mpu.dmpInitialize();
-
- // make sure it worked (returns 0 if so)
- if (devStatus == 0) {
- // turn on the DMP, now that it's ready
- pc.printf("Enabling DMP...\r\n");
- mpu.setDMPEnabled(true);
-
- // enable Arduino interrupt detection
- pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
- checkpin.rise(&dmpDataReady);
-
- mpuIntStatus = mpu.getIntStatus();
-
- // set our DMP Ready flag so the main loop() function knows it's okay to use it
- pc.printf("DMP ready! Waiting for first interrupt...\r\n");
- dmpReady = true;
-
- // get expected DMP packet size for later comparison
- packetSize = mpu.dmpGetFIFOPacketSize();
- } else {
- // ERROR!
- // 1 = initial memory load failed
- // 2 = DMP configuration updates failed
- // (if it's going to break, usually the code will be 1)
-
- pc.printf("DDMP Initialization failed (code ");
- pc.printf("%d", devStatus);
- pc.printf(")\r\n");
- }
-
-}
-
-
-
-// ================================================================
-// === MAIN PROGRAM LOOP ===
-// ================================================================
-
-void loop() {
- // if programming failed, don't try to do anything
- if (!dmpReady) return;
-
- // wait for MPU interrupt or extra packet(s) available
- while (!mpuInterrupt && fifoCount < packetSize) {
- // other program behavior stuff here
- // .
- // .
- // .
- // if you are really paranoid you can frequently test in between other
- // stuff to see if mpuInterrupt is true, and if so, "break;" from the
- // while() loop to immediately process the MPU data
- // .
- // .
- // .
- }
-
- // reset interrupt flag and get INT_STATUS byte
- mpuInterrupt = false;
- mpuIntStatus = mpu.getIntStatus();
-
- // get current FIFO count
- fifoCount = mpu.getFIFOCount();
-
- // check for overflow (this should never happen unless our code is too inefficient)
- if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
- // reset so we can continue cleanly
- mpu.resetFIFO();
- //Serial.println(F("FIFO overflow!"));
-
- // otherwise, check for DMP data ready interrupt (this should happen frequently)
- } else if (mpuIntStatus & 0x02) {
- // wait for correct available data length, should be a VERY short wait
- while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
-
- // read a packet from FIFO
- mpu.getFIFOBytes(fifoBuffer, packetSize);
-
- // track FIFO count here in case there is > 1 packet available
- // (this lets us immediately read more without waiting for an interrupt)
- fifoCount -= packetSize;
-
- #ifdef OUTPUT_READABLE_QUATERNION
- // display quaternion values in easy matrix form: w x y z
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- printf("quat\t");
- printf("%f\t", q.w);
- printf("%f\t", q.x);
- printf("%f\t", q.y);
- printf("%f\t\r\n", q.z);
- #endif
-
- #ifdef OUTPUT_READABLE_EULER
- // display Euler angles in degrees
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetEuler(euler, &q);
- printf("euler\t");
- printf("%f\t", euler[0] * 180/M_PI);
- printf("%f\t", euler[1] * 180/M_PI);
- printf("%f\t\r\n", euler[2] * 180/M_PI);
- #endif
-
- #ifdef OUTPUT_READABLE_YAWPITCHROLL
- // display Euler angles in degrees
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
- printf("ypr\t");
- printf("%f\t", ypr[0] * 180/M_PI);
- printf("%f\t", ypr[1] * 180/M_PI);
- printf("%f\t\r\n", ypr[2] * 180/M_PI);
- #endif
-
- #ifdef OUTPUT_READABLE_REALACCEL
- // display real acceleration, adjusted to remove gravity
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetAccel(&aa, fifoBuffer);
- mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
- printf("areal\t");
- printf("%f\t", aaReal.x);
- printf("%f\t", aaReal.y);
- printf("%f\t\r\n", aaReal.z);
- #endif
-
- #ifdef OUTPUT_READABLE_WORLDACCEL
- // display initial world-frame acceleration, adjusted to remove gravity
- // and rotated based on known orientation from quaternion
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetAccel(&aa, fifoBuffer);
- mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
- printf("aworld\t");
- printf("%f\t", aaWorld.x);
- printf("%f\t", aaWorld.y);
- printf("%f\t\r\n", aaWorld.z);
- #endif
-
- #ifdef OUTPUT_TEAPOT
- // display quaternion values in InvenSense Teapot demo format:
- teapotPacket[2] = fifoBuffer[0];
- teapotPacket[3] = fifoBuffer[1];
- teapotPacket[4] = fifoBuffer[4];
- teapotPacket[5] = fifoBuffer[5];
- teapotPacket[6] = fifoBuffer[8];
- teapotPacket[7] = fifoBuffer[9];
- teapotPacket[8] = fifoBuffer[12];
- teapotPacket[9] = fifoBuffer[13];
- for (int i = 0; i < 14; ++i) {
- pc.send(teapotPacket[i]);
- }
- teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
- #endif
-
- // blink LED to indicate activity
- led1 = !led1;
+ MPU6050raw::loop();
}
}
\ No newline at end of file