From 38695a0c766a860e6d45207619b30f1693efe51c Mon Sep 17 00:00:00 2001 From: unknown Date: Fri, 8 Jan 2016 10:50:24 -0500 Subject: [PATCH] changed animation --- .../MPU6050_Multi/MPU6050_Multi.ino | 146 ++++++++++++++++++ VRIS-prototype/Xylos.exe.lnk | Bin 2072 -> 2126 bytes 2 files changed, 146 insertions(+) create mode 100644 VRIS-prototype/Source/VRISprototype-external/MPU6050_Multi/MPU6050_Multi.ino diff --git a/VRIS-prototype/Source/VRISprototype-external/MPU6050_Multi/MPU6050_Multi.ino b/VRIS-prototype/Source/VRISprototype-external/MPU6050_Multi/MPU6050_Multi.ino new file mode 100644 index 0000000..4f5fa7a --- /dev/null +++ b/VRIS-prototype/Source/VRISprototype-external/MPU6050_Multi/MPU6050_Multi.ino @@ -0,0 +1,146 @@ +#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" + +#define NUMBER_OF_SENSORS 3 /// YOU MAY NEED TO CHANGE THIS + +// Default I2C address is 0x68 +// AD0 LOW(0) = 0x68 (Default for SparkFun breakout and InvenSense evaluation board) +// AD0 HIGH(1) = 0x69 + +// MPU Control Variables +MPU6050 mpu; +bool dmpReady; // Set true if DMP init was successful. +uint8_t devStatus; // Return status after each device operation. (0 = success, !0 = error) +uint8_t mpuIntStatus; // Holds interrupt status byte from MPU. +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 and Motion Variables +Quaternion q; // [W, X, Y, Z] Quaternion container. +VectorFloat gravity; // [X, Y, Z] Gravity vector +float ypr[3]; // [Yaw, Pitch, Roll] array container. + +//Digital Pins Reference Variables +/* Displace array beginning with 0. + * The numbers after the 0th index correspond to the pin connected to the ADO line of each sensor. + */ +int sensorPins[NUMBER_OF_SENSORS + 1] = {0, 5, 6, 7}; // {none, Sensor 1 ADO pin, Sensor 2 ADO pin,...} + +// Other Variables +String finalParts[NUMBER_OF_SENSORS]; // Final output. +String final = ""; // Part of final output. + +//============================================================== + +void switchSensor(int sensorNumber) { + digitalWrite(sensorPins[sensorNumber], LOW); // Change one ADO line to low. (The one we are recieving from! 0x68) +} + +// ================================================================ +// === MAIN PROGRAM SETUP === +// ================================================================ + +void setup() { + + // Setup Variables + int i = 0; // For loop counter. + int dmpReadyCounter = 0; // Counts number of sensors ready. + + // Set all ADO lines to default high, and configure pins. + for(i = 1; i <= NUMBER_OF_SENSORS; i++) { + pinMode(sensorPins[i], OUTPUT); + digitalWrite(sensorPins[i], HIGH); + } + + Serial.begin(115200); // Initialize serial communication with baud rate. + + for (i = 1; i <= NUMBER_OF_SENSORS; i++) { + /* We read data from all sensors by switching addresses one by one, only reading from the first address (0x68). + * Therefore, we select sensors by making an ADO line LOW. + */ + switchSensor(i); + + Wire.begin(); // Joins I2C bus. (I2C libary doesn't do this on it's own.) + TWBR = 24; // Sets SCL higher. + + mpu.initialize(); // Intialize device. + + devStatus = mpu.dmpInitialize(); // Load and configure the DMP. (Digital Motion Processor) + + // Gyroscope offsets. (Change if necessary) + mpu.setXGyroOffset(220); + mpu.setYGyroOffset(76); + mpu.setZGyroOffset(-85); + mpu.setZAccelOffset(1788); + + // Check success of DMP. + if (devStatus == 0) { + mpu.setDMPEnabled(true); + + dmpReadyCounter += 1; // Add one to count number of ready sensors. + + packetSize = mpu.dmpGetFIFOPacketSize(); // Get expected DMP packet size for later comparison + + } else { + // Error! + Serial.println("Error on sensor " + String(i)); + } + digitalWrite(sensorPins[i], HIGH); // Reset current ADO pin back to high. (0x69) + Wire.endTransmission(); // End transmission for sensor. + } + if (dmpReadyCounter == NUMBER_OF_SENSORS) { + dmpReady = true; // Set DMP Ready flag. (Allows main loop to use the DMP.) + } +} + +// ================================================================ +// === MAIN PROGRAM LOOP === +// ================================================================ + +void loop() { + // If DMP isn't ready... + if (!(dmpReady)) { + return; + } + + final = ""; // Reset final to nothing. + + for (int i = 1; i <= NUMBER_OF_SENSORS; i++) { + + switchSensor(i); + + // Check for overflow. + if (fifoCount == 1024) { + mpu.resetFIFO(); // Reset so we can continue cleanly. + } else { + fifoCount = mpu.getFIFOCount(); // Get current FIFO count. + + // Wait for correct avaliable data length. + while (fifoCount < packetSize) { + fifoCount = mpu.getFIFOCount(); + } + + mpu.getFIFOBytes(fifoBuffer, packetSize); // Read a packet from FIFO + /* Track FIFO count in case there is more than 1 packet avaliable. + * (Read more without waiting for an interrupt.) + */ + fifoCount -= packetSize; + + // Get values to process. + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + + // Concatenate for outputting. (Displays in Euler Angles in degrees.) + finalParts[i - 1] = String(ypr[0] * 180 / M_PI) + "," + String(ypr[1] * 180 / M_PI) + "," + String(ypr[2] * 180 / M_PI) + ":"; + final += finalParts[i - 1]; + digitalWrite(sensorPins[i], HIGH); // Resets current ADO pin back to high. (0x69) + } + } + Serial.println(final.substring(0, final.length() - 1)); // Prints and removes extra colon. +} diff --git a/VRIS-prototype/Xylos.exe.lnk b/VRIS-prototype/Xylos.exe.lnk index d9b710208803bab1fa4e4db7f51c413172cdb73f..66887b9a2d15c52b9cc33716887f1773e8717921 100644 GIT binary patch delta 72 zcmbOsa86*uS7ye_$={gwnkq2pGNdx(GL$mpFeEaR0NKT0vIxk^X8_5jf#q_5;>ipk N(Nu=K&6`-#m;u?75=8(2 delta 18 acmX>nFhgL&S7yez$={gwZhpy<$_xNaR0rPx