diff --git a/CAD/DEC_mount.stl b/CAD/DEC_mount.stl new file mode 100644 index 0000000..bd18c07 Binary files /dev/null and b/CAD/DEC_mount.stl differ diff --git a/CAD/base.stl b/CAD/base.stl new file mode 100644 index 0000000..1e7fd43 Binary files /dev/null and b/CAD/base.stl differ diff --git a/CAD/base_gear_big.stl b/CAD/base_gear_big.stl new file mode 100644 index 0000000..3aa3b95 Binary files /dev/null and b/CAD/base_gear_big.stl differ diff --git a/CAD/base_gear_small.stl b/CAD/base_gear_small.stl new file mode 100644 index 0000000..d5d6f95 Binary files /dev/null and b/CAD/base_gear_small.stl differ diff --git a/CAD/base_screw.stl b/CAD/base_screw.stl new file mode 100644 index 0000000..825bf57 Binary files /dev/null and b/CAD/base_screw.stl differ diff --git a/CAD/first_gear_DEC.stl b/CAD/first_gear_DEC.stl new file mode 100644 index 0000000..ead49b9 Binary files /dev/null and b/CAD/first_gear_DEC.stl differ diff --git a/CAD/gyro_mount.stl b/CAD/gyro_mount.stl new file mode 100644 index 0000000..03f7fc2 Binary files /dev/null and b/CAD/gyro_mount.stl differ diff --git a/CAD/laser_mount.stl b/CAD/laser_mount.stl new file mode 100644 index 0000000..e133aad Binary files /dev/null and b/CAD/laser_mount.stl differ diff --git a/CAD/second_gear_DEC.stl b/CAD/second_gear_DEC.stl new file mode 100644 index 0000000..088ddee Binary files /dev/null and b/CAD/second_gear_DEC.stl differ diff --git a/CAD/small_gear_DEC.stl b/CAD/small_gear_DEC.stl new file mode 100644 index 0000000..df2a674 Binary files /dev/null and b/CAD/small_gear_DEC.stl differ diff --git a/CAD/tripod_mount.stl b/CAD/tripod_mount.stl new file mode 100644 index 0000000..56042c5 Binary files /dev/null and b/CAD/tripod_mount.stl differ diff --git a/Firmware/MPU6050/MPU6050.cpp b/Firmware/MPU6050/MPU6050.cpp new file mode 100644 index 0000000..d9cc131 --- /dev/null +++ b/Firmware/MPU6050/MPU6050.cpp @@ -0,0 +1,749 @@ +/* +MPU6050.cpp - Class file for the MPU6050 Triple Axis Gyroscope & Accelerometer Arduino Library. + +Version: 1.0.3 +(c) 2014-2015 Korneliusz Jarzebski +www.jarzebski.pl + +This program is free software: you can redistribute it and/or modify +it under the terms of the version 3 GNU General Public License as +published by the Free Software Foundation. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +*/ + +#if ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +#include +#include + +#include + +bool MPU6050::begin(mpu6050_dps_t scale, mpu6050_range_t range, int mpua) +{ + // Set Address + mpuAddress = mpua; + + Wire.begin(); + + // Reset calibrate values + dg.XAxis = 0; + dg.YAxis = 0; + dg.ZAxis = 0; + useCalibrate = false; + + // Reset threshold values + tg.XAxis = 0; + tg.YAxis = 0; + tg.ZAxis = 0; + actualThreshold = 0; + + // Check MPU6050 Who Am I Register + if (fastRegister8(MPU6050_REG_WHO_AM_I) != 0x68) + { + return false; + } + + // Set Clock Source + setClockSource(MPU6050_CLOCK_PLL_XGYRO); + + // Set Scale & Range + setScale(scale); + setRange(range); + + // Disable Sleep Mode + setSleepEnabled(false); + + return true; +} + +void MPU6050::setScale(mpu6050_dps_t scale) +{ + uint8_t value; + + switch (scale) + { + case MPU6050_SCALE_250DPS: + dpsPerDigit = .007633f; + break; + case MPU6050_SCALE_500DPS: + dpsPerDigit = .015267f; + break; + case MPU6050_SCALE_1000DPS: + dpsPerDigit = .030487f; + break; + case MPU6050_SCALE_2000DPS: + dpsPerDigit = .060975f; + break; + default: + break; + } + + value = readRegister8(MPU6050_REG_GYRO_CONFIG); + value &= 0b11100111; + value |= (scale << 3); + writeRegister8(MPU6050_REG_GYRO_CONFIG, value); +} + +mpu6050_dps_t MPU6050::getScale(void) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_GYRO_CONFIG); + value &= 0b00011000; + value >>= 3; + return (mpu6050_dps_t)value; +} + +void MPU6050::setRange(mpu6050_range_t range) +{ + uint8_t value; + + switch (range) + { + case MPU6050_RANGE_2G: + rangePerDigit = .000061f; + break; + case MPU6050_RANGE_4G: + rangePerDigit = .000122f; + break; + case MPU6050_RANGE_8G: + rangePerDigit = .000244f; + break; + case MPU6050_RANGE_16G: + rangePerDigit = .0004882f; + break; + default: + break; + } + + value = readRegister8(MPU6050_REG_ACCEL_CONFIG); + value &= 0b11100111; + value |= (range << 3); + writeRegister8(MPU6050_REG_ACCEL_CONFIG, value); +} + +mpu6050_range_t MPU6050::getRange(void) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_ACCEL_CONFIG); + value &= 0b00011000; + value >>= 3; + return (mpu6050_range_t)value; +} + +void MPU6050::setDHPFMode(mpu6050_dhpf_t dhpf) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_ACCEL_CONFIG); + value &= 0b11111000; + value |= dhpf; + writeRegister8(MPU6050_REG_ACCEL_CONFIG, value); +} + +void MPU6050::setDLPFMode(mpu6050_dlpf_t dlpf) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_CONFIG); + value &= 0b11111000; + value |= dlpf; + writeRegister8(MPU6050_REG_CONFIG, value); +} + +void MPU6050::setClockSource(mpu6050_clockSource_t source) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_PWR_MGMT_1); + value &= 0b11111000; + value |= source; + writeRegister8(MPU6050_REG_PWR_MGMT_1, value); +} + +mpu6050_clockSource_t MPU6050::getClockSource(void) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_PWR_MGMT_1); + value &= 0b00000111; + return (mpu6050_clockSource_t)value; +} + +bool MPU6050::getSleepEnabled(void) +{ + return readRegisterBit(MPU6050_REG_PWR_MGMT_1, 6); +} + +void MPU6050::setSleepEnabled(bool state) +{ + writeRegisterBit(MPU6050_REG_PWR_MGMT_1, 6, state); +} + +bool MPU6050::getIntZeroMotionEnabled(void) +{ + return readRegisterBit(MPU6050_REG_INT_ENABLE, 5); +} + +void MPU6050::setIntZeroMotionEnabled(bool state) +{ + writeRegisterBit(MPU6050_REG_INT_ENABLE, 5, state); +} + +bool MPU6050::getIntMotionEnabled(void) +{ + return readRegisterBit(MPU6050_REG_INT_ENABLE, 6); +} + +void MPU6050::setIntMotionEnabled(bool state) +{ + writeRegisterBit(MPU6050_REG_INT_ENABLE, 6, state); +} + +bool MPU6050::getIntFreeFallEnabled(void) +{ + return readRegisterBit(MPU6050_REG_INT_ENABLE, 7); +} + +void MPU6050::setIntFreeFallEnabled(bool state) +{ + writeRegisterBit(MPU6050_REG_INT_ENABLE, 7, state); +} + +uint8_t MPU6050::getMotionDetectionThreshold(void) +{ + return readRegister8(MPU6050_REG_MOT_THRESHOLD); +} + +void MPU6050::setMotionDetectionThreshold(uint8_t threshold) +{ + writeRegister8(MPU6050_REG_MOT_THRESHOLD, threshold); +} + +uint8_t MPU6050::getMotionDetectionDuration(void) +{ + return readRegister8(MPU6050_REG_MOT_DURATION); +} + +void MPU6050::setMotionDetectionDuration(uint8_t duration) +{ + writeRegister8(MPU6050_REG_MOT_DURATION, duration); +} + +uint8_t MPU6050::getZeroMotionDetectionThreshold(void) +{ + return readRegister8(MPU6050_REG_ZMOT_THRESHOLD); +} + +void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) +{ + writeRegister8(MPU6050_REG_ZMOT_THRESHOLD, threshold); +} + +uint8_t MPU6050::getZeroMotionDetectionDuration(void) +{ + return readRegister8(MPU6050_REG_ZMOT_DURATION); +} + +void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) +{ + writeRegister8(MPU6050_REG_ZMOT_DURATION, duration); +} + +uint8_t MPU6050::getFreeFallDetectionThreshold(void) +{ + return readRegister8(MPU6050_REG_FF_THRESHOLD); +} + +void MPU6050::setFreeFallDetectionThreshold(uint8_t threshold) +{ + writeRegister8(MPU6050_REG_FF_THRESHOLD, threshold); +} + +uint8_t MPU6050::getFreeFallDetectionDuration(void) +{ + return readRegister8(MPU6050_REG_FF_DURATION); +} + +void MPU6050::setFreeFallDetectionDuration(uint8_t duration) +{ + writeRegister8(MPU6050_REG_FF_DURATION, duration); +} + +bool MPU6050::getI2CMasterModeEnabled(void) +{ + return readRegisterBit(MPU6050_REG_USER_CTRL, 5); +} + +void MPU6050::setI2CMasterModeEnabled(bool state) +{ + writeRegisterBit(MPU6050_REG_USER_CTRL, 5, state); +} + +void MPU6050::setI2CBypassEnabled(bool state) +{ + return writeRegisterBit(MPU6050_REG_INT_PIN_CFG, 1, state); +} + +bool MPU6050::getI2CBypassEnabled(void) +{ + return readRegisterBit(MPU6050_REG_INT_PIN_CFG, 1); +} + +void MPU6050::setAccelPowerOnDelay(mpu6050_onDelay_t delay) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_MOT_DETECT_CTRL); + value &= 0b11001111; + value |= (delay << 4); + writeRegister8(MPU6050_REG_MOT_DETECT_CTRL, value); +} + +mpu6050_onDelay_t MPU6050::getAccelPowerOnDelay(void) +{ + uint8_t value; + value = readRegister8(MPU6050_REG_MOT_DETECT_CTRL); + value &= 0b00110000; + return (mpu6050_onDelay_t)(value >> 4); +} + +uint8_t MPU6050::getIntStatus(void) +{ + return readRegister8(MPU6050_REG_INT_STATUS); +} + +Activites MPU6050::readActivites(void) +{ + uint8_t data = readRegister8(MPU6050_REG_INT_STATUS); + + a.isOverflow = ((data >> 4) & 1); + a.isFreeFall = ((data >> 7) & 1); + a.isInactivity = ((data >> 5) & 1); + a.isActivity = ((data >> 6) & 1); + a.isDataReady = ((data >> 0) & 1); + + data = readRegister8(MPU6050_REG_MOT_DETECT_STATUS); + + a.isNegActivityOnX = ((data >> 7) & 1); + a.isPosActivityOnX = ((data >> 6) & 1); + + a.isNegActivityOnY = ((data >> 5) & 1); + a.isPosActivityOnY = ((data >> 4) & 1); + + a.isNegActivityOnZ = ((data >> 3) & 1); + a.isPosActivityOnZ = ((data >> 2) & 1); + + return a; +} + +Vector MPU6050::readRawAccel(void) +{ + Wire.beginTransmission(mpuAddress); + #if ARDUINO >= 100 + Wire.write(MPU6050_REG_ACCEL_XOUT_H); + #else + Wire.send(MPU6050_REG_ACCEL_XOUT_H); + #endif + Wire.endTransmission(); + + Wire.beginTransmission(mpuAddress); + Wire.requestFrom(mpuAddress, 6); + + while (Wire.available() < 6); + + #if ARDUINO >= 100 + uint8_t xha = Wire.read(); + uint8_t xla = Wire.read(); + uint8_t yha = Wire.read(); + uint8_t yla = Wire.read(); + uint8_t zha = Wire.read(); + uint8_t zla = Wire.read(); + #else + uint8_t xha = Wire.receive(); + uint8_t xla = Wire.receive(); + uint8_t yha = Wire.receive(); + uint8_t yla = Wire.receive(); + uint8_t zha = Wire.receive(); + uint8_t zla = Wire.receive(); + #endif + + ra.XAxis = xha << 8 | xla; + ra.YAxis = yha << 8 | yla; + ra.ZAxis = zha << 8 | zla; + + return ra; +} + +Vector MPU6050::readNormalizeAccel(void) +{ + readRawAccel(); + + na.XAxis = ra.XAxis * rangePerDigit * 9.80665f; + na.YAxis = ra.YAxis * rangePerDigit * 9.80665f; + na.ZAxis = ra.ZAxis * rangePerDigit * 9.80665f; + + return na; +} + +Vector MPU6050::readScaledAccel(void) +{ + readRawAccel(); + + na.XAxis = ra.XAxis * rangePerDigit; + na.YAxis = ra.YAxis * rangePerDigit; + na.ZAxis = ra.ZAxis * rangePerDigit; + + return na; +} + + +Vector MPU6050::readRawGyro(void) +{ + Wire.beginTransmission(mpuAddress); + #if ARDUINO >= 100 + Wire.write(MPU6050_REG_GYRO_XOUT_H); + #else + Wire.send(MPU6050_REG_GYRO_XOUT_H); + #endif + Wire.endTransmission(); + + Wire.beginTransmission(mpuAddress); + Wire.requestFrom(mpuAddress, 6); + + while (Wire.available() < 6); + + #if ARDUINO >= 100 + uint8_t xha = Wire.read(); + uint8_t xla = Wire.read(); + uint8_t yha = Wire.read(); + uint8_t yla = Wire.read(); + uint8_t zha = Wire.read(); + uint8_t zla = Wire.read(); + #else + uint8_t xha = Wire.receive(); + uint8_t xla = Wire.receive(); + uint8_t yha = Wire.receive(); + uint8_t yla = Wire.receive(); + uint8_t zha = Wire.receive(); + uint8_t zla = Wire.receive(); + #endif + + rg.XAxis = xha << 8 | xla; + rg.YAxis = yha << 8 | yla; + rg.ZAxis = zha << 8 | zla; + + return rg; +} + +Vector MPU6050::readNormalizeGyro(void) +{ + readRawGyro(); + + if (useCalibrate) + { + ng.XAxis = (rg.XAxis - dg.XAxis) * dpsPerDigit; + ng.YAxis = (rg.YAxis - dg.YAxis) * dpsPerDigit; + ng.ZAxis = (rg.ZAxis - dg.ZAxis) * dpsPerDigit; + } else + { + ng.XAxis = rg.XAxis * dpsPerDigit; + ng.YAxis = rg.YAxis * dpsPerDigit; + ng.ZAxis = rg.ZAxis * dpsPerDigit; + } + + if (actualThreshold) + { + if (abs(ng.XAxis) < tg.XAxis) ng.XAxis = 0; + if (abs(ng.YAxis) < tg.YAxis) ng.YAxis = 0; + if (abs(ng.ZAxis) < tg.ZAxis) ng.ZAxis = 0; + } + + return ng; +} + +float MPU6050::readTemperature(void) +{ + int16_t T; + T = readRegister16(MPU6050_REG_TEMP_OUT_H); + return (float)T/340 + 36.53; +} + +int16_t MPU6050::getGyroOffsetX(void) +{ + return readRegister16(MPU6050_REG_GYRO_XOFFS_H); +} + +int16_t MPU6050::getGyroOffsetY(void) +{ + return readRegister16(MPU6050_REG_GYRO_YOFFS_H); +} + +int16_t MPU6050::getGyroOffsetZ(void) +{ + return readRegister16(MPU6050_REG_GYRO_ZOFFS_H); +} + +void MPU6050::setGyroOffsetX(int16_t offset) +{ + writeRegister16(MPU6050_REG_GYRO_XOFFS_H, offset); +} + +void MPU6050::setGyroOffsetY(int16_t offset) +{ + writeRegister16(MPU6050_REG_GYRO_YOFFS_H, offset); +} + +void MPU6050::setGyroOffsetZ(int16_t offset) +{ + writeRegister16(MPU6050_REG_GYRO_ZOFFS_H, offset); +} + +int16_t MPU6050::getAccelOffsetX(void) +{ + return readRegister16(MPU6050_REG_ACCEL_XOFFS_H); +} + +int16_t MPU6050::getAccelOffsetY(void) +{ + return readRegister16(MPU6050_REG_ACCEL_YOFFS_H); +} + +int16_t MPU6050::getAccelOffsetZ(void) +{ + return readRegister16(MPU6050_REG_ACCEL_ZOFFS_H); +} + +void MPU6050::setAccelOffsetX(int16_t offset) +{ + writeRegister16(MPU6050_REG_ACCEL_XOFFS_H, offset); +} + +void MPU6050::setAccelOffsetY(int16_t offset) +{ + writeRegister16(MPU6050_REG_ACCEL_YOFFS_H, offset); +} + +void MPU6050::setAccelOffsetZ(int16_t offset) +{ + writeRegister16(MPU6050_REG_ACCEL_ZOFFS_H, offset); +} + +// Calibrate algorithm +void MPU6050::calibrateGyro(uint8_t samples) +{ + // Set calibrate + useCalibrate = true; + + // Reset values + float sumX = 0; + float sumY = 0; + float sumZ = 0; + float sigmaX = 0; + float sigmaY = 0; + float sigmaZ = 0; + + // Read n-samples + for (uint8_t i = 0; i < samples; ++i) + { + readRawGyro(); + sumX += rg.XAxis; + sumY += rg.YAxis; + sumZ += rg.ZAxis; + + sigmaX += rg.XAxis * rg.XAxis; + sigmaY += rg.YAxis * rg.YAxis; + sigmaZ += rg.ZAxis * rg.ZAxis; + + delay(5); + } + + // Calculate delta vectors + dg.XAxis = sumX / samples; + dg.YAxis = sumY / samples; + dg.ZAxis = sumZ / samples; + + // Calculate threshold vectors + th.XAxis = sqrt((sigmaX / 50) - (dg.XAxis * dg.XAxis)); + th.YAxis = sqrt((sigmaY / 50) - (dg.YAxis * dg.YAxis)); + th.ZAxis = sqrt((sigmaZ / 50) - (dg.ZAxis * dg.ZAxis)); + + // If already set threshold, recalculate threshold vectors + if (actualThreshold > 0) + { + setThreshold(actualThreshold); + } +} + +// Get current threshold value +uint8_t MPU6050::getThreshold(void) +{ + return actualThreshold; +} + +// Set treshold value +void MPU6050::setThreshold(uint8_t multiple) +{ + if (multiple > 0) + { + // If not calibrated, need calibrate + if (!useCalibrate) + { + calibrateGyro(); + } + + // Calculate threshold vectors + tg.XAxis = th.XAxis * multiple; + tg.YAxis = th.YAxis * multiple; + tg.ZAxis = th.ZAxis * multiple; + } else + { + // No threshold + tg.XAxis = 0; + tg.YAxis = 0; + tg.ZAxis = 0; + } + + // Remember old threshold value + actualThreshold = multiple; +} + +// Fast read 8-bit from register +uint8_t MPU6050::fastRegister8(uint8_t reg) +{ + uint8_t value; + + Wire.beginTransmission(mpuAddress); + #if ARDUINO >= 100 + Wire.write(reg); + #else + Wire.send(reg); + #endif + Wire.endTransmission(); + + Wire.beginTransmission(mpuAddress); + Wire.requestFrom(mpuAddress, 1); + #if ARDUINO >= 100 + value = Wire.read(); + #else + value = Wire.receive(); + #endif; + Wire.endTransmission(); + + return value; +} + +// Read 8-bit from register +uint8_t MPU6050::readRegister8(uint8_t reg) +{ + uint8_t value; + + Wire.beginTransmission(mpuAddress); + #if ARDUINO >= 100 + Wire.write(reg); + #else + Wire.send(reg); + #endif + Wire.endTransmission(); + + Wire.beginTransmission(mpuAddress); + Wire.requestFrom(mpuAddress, 1); + while(!Wire.available()) {}; + #if ARDUINO >= 100 + value = Wire.read(); + #else + value = Wire.receive(); + #endif; + Wire.endTransmission(); + + return value; +} + +// Write 8-bit to register +void MPU6050::writeRegister8(uint8_t reg, uint8_t value) +{ + Wire.beginTransmission(mpuAddress); + + #if ARDUINO >= 100 + Wire.write(reg); + Wire.write(value); + #else + Wire.send(reg); + Wire.send(value); + #endif + Wire.endTransmission(); +} + +int16_t MPU6050::readRegister16(uint8_t reg) +{ + int16_t value; + Wire.beginTransmission(mpuAddress); + #if ARDUINO >= 100 + Wire.write(reg); + #else + Wire.send(reg); + #endif + Wire.endTransmission(); + + Wire.beginTransmission(mpuAddress); + Wire.requestFrom(mpuAddress, 2); + while(!Wire.available()) {}; + #if ARDUINO >= 100 + uint8_t vha = Wire.read(); + uint8_t vla = Wire.read(); + #else + uint8_t vha = Wire.receive(); + uint8_t vla = Wire.receive(); + #endif; + Wire.endTransmission(); + + value = vha << 8 | vla; + + return value; +} + +void MPU6050::writeRegister16(uint8_t reg, int16_t value) +{ + Wire.beginTransmission(mpuAddress); + + #if ARDUINO >= 100 + Wire.write(reg); + Wire.write((uint8_t)(value >> 8)); + Wire.write((uint8_t)value); + #else + Wire.send(reg); + Wire.send((uint8_t)(value >> 8)); + Wire.send((uint8_t)value); + #endif + Wire.endTransmission(); +} + +// Read register bit +bool MPU6050::readRegisterBit(uint8_t reg, uint8_t pos) +{ + uint8_t value; + value = readRegister8(reg); + return ((value >> pos) & 1); +} + +// Write register bit +void MPU6050::writeRegisterBit(uint8_t reg, uint8_t pos, bool state) +{ + uint8_t value; + value = readRegister8(reg); + + if (state) + { + value |= (1 << pos); + } else + { + value &= ~(1 << pos); + } + + writeRegister8(reg, value); +} diff --git a/Firmware/MPU6050/MPU6050.h b/Firmware/MPU6050/MPU6050.h new file mode 100644 index 0000000..fbf077e --- /dev/null +++ b/Firmware/MPU6050/MPU6050.h @@ -0,0 +1,258 @@ +/* +MPU6050.h - Header file for the MPU6050 Triple Axis Gyroscope & Accelerometer Arduino Library. + +Version: 1.0.3 +(c) 2014-2015 Korneliusz Jarzebski +www.jarzebski.pl + +This program is free software: you can redistribute it and/or modify +it under the terms of the version 3 GNU General Public License as +published by the Free Software Foundation. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +*/ + +#ifndef MPU6050_h +#define MPU6050_h + +#if ARDUINO >= 100 +#include "Arduino.h" +#else +#include "WProgram.h" +#endif + +#define MPU6050_ADDRESS (0x68) // 0x69 when AD0 pin to Vcc + +#define MPU6050_REG_ACCEL_XOFFS_H (0x06) +#define MPU6050_REG_ACCEL_XOFFS_L (0x07) +#define MPU6050_REG_ACCEL_YOFFS_H (0x08) +#define MPU6050_REG_ACCEL_YOFFS_L (0x09) +#define MPU6050_REG_ACCEL_ZOFFS_H (0x0A) +#define MPU6050_REG_ACCEL_ZOFFS_L (0x0B) +#define MPU6050_REG_GYRO_XOFFS_H (0x13) +#define MPU6050_REG_GYRO_XOFFS_L (0x14) +#define MPU6050_REG_GYRO_YOFFS_H (0x15) +#define MPU6050_REG_GYRO_YOFFS_L (0x16) +#define MPU6050_REG_GYRO_ZOFFS_H (0x17) +#define MPU6050_REG_GYRO_ZOFFS_L (0x18) +#define MPU6050_REG_CONFIG (0x1A) +#define MPU6050_REG_GYRO_CONFIG (0x1B) // Gyroscope Configuration +#define MPU6050_REG_ACCEL_CONFIG (0x1C) // Accelerometer Configuration +#define MPU6050_REG_FF_THRESHOLD (0x1D) +#define MPU6050_REG_FF_DURATION (0x1E) +#define MPU6050_REG_MOT_THRESHOLD (0x1F) +#define MPU6050_REG_MOT_DURATION (0x20) +#define MPU6050_REG_ZMOT_THRESHOLD (0x21) +#define MPU6050_REG_ZMOT_DURATION (0x22) +#define MPU6050_REG_INT_PIN_CFG (0x37) // INT Pin. Bypass Enable Configuration +#define MPU6050_REG_INT_ENABLE (0x38) // INT Enable +#define MPU6050_REG_INT_STATUS (0x3A) +#define MPU6050_REG_ACCEL_XOUT_H (0x3B) +#define MPU6050_REG_ACCEL_XOUT_L (0x3C) +#define MPU6050_REG_ACCEL_YOUT_H (0x3D) +#define MPU6050_REG_ACCEL_YOUT_L (0x3E) +#define MPU6050_REG_ACCEL_ZOUT_H (0x3F) +#define MPU6050_REG_ACCEL_ZOUT_L (0x40) +#define MPU6050_REG_TEMP_OUT_H (0x41) +#define MPU6050_REG_TEMP_OUT_L (0x42) +#define MPU6050_REG_GYRO_XOUT_H (0x43) +#define MPU6050_REG_GYRO_XOUT_L (0x44) +#define MPU6050_REG_GYRO_YOUT_H (0x45) +#define MPU6050_REG_GYRO_YOUT_L (0x46) +#define MPU6050_REG_GYRO_ZOUT_H (0x47) +#define MPU6050_REG_GYRO_ZOUT_L (0x48) +#define MPU6050_REG_MOT_DETECT_STATUS (0x61) +#define MPU6050_REG_MOT_DETECT_CTRL (0x69) +#define MPU6050_REG_USER_CTRL (0x6A) // User Control +#define MPU6050_REG_PWR_MGMT_1 (0x6B) // Power Management 1 +#define MPU6050_REG_WHO_AM_I (0x75) // Who Am I + +#ifndef VECTOR_STRUCT_H +#define VECTOR_STRUCT_H +struct Vector +{ + float XAxis; + float YAxis; + float ZAxis; +}; +#endif + +struct Activites +{ + bool isOverflow; + bool isFreeFall; + bool isInactivity; + bool isActivity; + bool isPosActivityOnX; + bool isPosActivityOnY; + bool isPosActivityOnZ; + bool isNegActivityOnX; + bool isNegActivityOnY; + bool isNegActivityOnZ; + bool isDataReady; +}; + +typedef enum +{ + MPU6050_CLOCK_KEEP_RESET = 0b111, + MPU6050_CLOCK_EXTERNAL_19MHZ = 0b101, + MPU6050_CLOCK_EXTERNAL_32KHZ = 0b100, + MPU6050_CLOCK_PLL_ZGYRO = 0b011, + MPU6050_CLOCK_PLL_YGYRO = 0b010, + MPU6050_CLOCK_PLL_XGYRO = 0b001, + MPU6050_CLOCK_INTERNAL_8MHZ = 0b000 +} mpu6050_clockSource_t; + +typedef enum +{ + MPU6050_SCALE_2000DPS = 0b11, + MPU6050_SCALE_1000DPS = 0b10, + MPU6050_SCALE_500DPS = 0b01, + MPU6050_SCALE_250DPS = 0b00 +} mpu6050_dps_t; + +typedef enum +{ + MPU6050_RANGE_16G = 0b11, + MPU6050_RANGE_8G = 0b10, + MPU6050_RANGE_4G = 0b01, + MPU6050_RANGE_2G = 0b00, +} mpu6050_range_t; + +typedef enum +{ + MPU6050_DELAY_3MS = 0b11, + MPU6050_DELAY_2MS = 0b10, + MPU6050_DELAY_1MS = 0b01, + MPU6050_NO_DELAY = 0b00, +} mpu6050_onDelay_t; + +typedef enum +{ + MPU6050_DHPF_HOLD = 0b111, + MPU6050_DHPF_0_63HZ = 0b100, + MPU6050_DHPF_1_25HZ = 0b011, + MPU6050_DHPF_2_5HZ = 0b010, + MPU6050_DHPF_5HZ = 0b001, + MPU6050_DHPF_RESET = 0b000, +} mpu6050_dhpf_t; + +typedef enum +{ + MPU6050_DLPF_6 = 0b110, + MPU6050_DLPF_5 = 0b101, + MPU6050_DLPF_4 = 0b100, + MPU6050_DLPF_3 = 0b011, + MPU6050_DLPF_2 = 0b010, + MPU6050_DLPF_1 = 0b001, + MPU6050_DLPF_0 = 0b000, +} mpu6050_dlpf_t; + +class MPU6050 +{ + public: + + bool begin(mpu6050_dps_t scale = MPU6050_SCALE_2000DPS, mpu6050_range_t range = MPU6050_RANGE_2G, int mpua = MPU6050_ADDRESS); + + void setClockSource(mpu6050_clockSource_t source); + void setScale(mpu6050_dps_t scale); + void setRange(mpu6050_range_t range); + mpu6050_clockSource_t getClockSource(void); + mpu6050_dps_t getScale(void); + mpu6050_range_t getRange(void); + void setDHPFMode(mpu6050_dhpf_t dhpf); + void setDLPFMode(mpu6050_dlpf_t dlpf); + mpu6050_onDelay_t getAccelPowerOnDelay(); + void setAccelPowerOnDelay(mpu6050_onDelay_t delay); + + uint8_t getIntStatus(void); + + bool getIntZeroMotionEnabled(void); + void setIntZeroMotionEnabled(bool state); + bool getIntMotionEnabled(void); + void setIntMotionEnabled(bool state); + bool getIntFreeFallEnabled(void); + void setIntFreeFallEnabled(bool state); + + uint8_t getMotionDetectionThreshold(void); + void setMotionDetectionThreshold(uint8_t threshold); + uint8_t getMotionDetectionDuration(void); + void setMotionDetectionDuration(uint8_t duration); + + uint8_t getZeroMotionDetectionThreshold(void); + void setZeroMotionDetectionThreshold(uint8_t threshold); + uint8_t getZeroMotionDetectionDuration(void); + void setZeroMotionDetectionDuration(uint8_t duration); + + uint8_t getFreeFallDetectionThreshold(void); + void setFreeFallDetectionThreshold(uint8_t threshold); + uint8_t getFreeFallDetectionDuration(void); + void setFreeFallDetectionDuration(uint8_t duration); + + bool getSleepEnabled(void); + void setSleepEnabled(bool state); + bool getI2CMasterModeEnabled(void); + void setI2CMasterModeEnabled(bool state); + bool getI2CBypassEnabled(void); + void setI2CBypassEnabled(bool state); + + float readTemperature(void); + Activites readActivites(void); + + int16_t getGyroOffsetX(void); + void setGyroOffsetX(int16_t offset); + int16_t getGyroOffsetY(void); + void setGyroOffsetY(int16_t offset); + int16_t getGyroOffsetZ(void); + void setGyroOffsetZ(int16_t offset); + + int16_t getAccelOffsetX(void); + void setAccelOffsetX(int16_t offset); + int16_t getAccelOffsetY(void); + void setAccelOffsetY(int16_t offset); + int16_t getAccelOffsetZ(void); + void setAccelOffsetZ(int16_t offset); + + void calibrateGyro(uint8_t samples = 50); + void setThreshold(uint8_t multiple = 1); + uint8_t getThreshold(void); + + Vector readRawGyro(void); + Vector readNormalizeGyro(void); + + Vector readRawAccel(void); + Vector readNormalizeAccel(void); + Vector readScaledAccel(void); + + private: + Vector ra, rg; // Raw vectors + Vector na, ng; // Normalized vectors + Vector tg, dg; // Threshold and Delta for Gyro + Vector th; // Threshold + Activites a; // Activities + + float dpsPerDigit, rangePerDigit; + float actualThreshold; + bool useCalibrate; + int mpuAddress; + + uint8_t fastRegister8(uint8_t reg); + + uint8_t readRegister8(uint8_t reg); + void writeRegister8(uint8_t reg, uint8_t value); + + int16_t readRegister16(uint8_t reg); + void writeRegister16(uint8_t reg, int16_t value); + + bool readRegisterBit(uint8_t reg, uint8_t pos); + void writeRegisterBit(uint8_t reg, uint8_t pos, bool state); + +}; + +#endif diff --git a/Firmware/MPU6050/MPU6050.pdf b/Firmware/MPU6050/MPU6050.pdf new file mode 100644 index 0000000..8e5ac97 Binary files /dev/null and b/Firmware/MPU6050/MPU6050.pdf differ diff --git a/Firmware/MPU6050/README.md b/Firmware/MPU6050/README.md new file mode 100644 index 0000000..e612c1a --- /dev/null +++ b/Firmware/MPU6050/README.md @@ -0,0 +1,10 @@ +Arduino-MPU6050 +=============== + +MPU6050 Triple Axis Gyroscope & Accelerometer Arduino Library. + +![MPU6050 Processing](http://www.jarzebski.pl/media/zoom/publish/2014/10/mpu6050-processing-2.png "MPU6050 Processing") + +Tutorials: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html + +This library use I2C to communicate, 2 pins are required to interface diff --git a/Firmware/MPU6050/examples/MPU6050_accel_pitch_roll/MPU6050_accel_pitch_roll.ino b/Firmware/MPU6050/examples/MPU6050_accel_pitch_roll/MPU6050_accel_pitch_roll.ino new file mode 100644 index 0000000..c1b0e2f --- /dev/null +++ b/Firmware/MPU6050/examples/MPU6050_accel_pitch_roll/MPU6050_accel_pitch_roll.ino @@ -0,0 +1,47 @@ +/* + MPU6050 Triple Axis Gyroscope & Accelerometer. Pitch & Roll Accelerometer Example. + Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html + GIT: https://github.com/jarzebski/Arduino-MPU6050 + Web: http://www.jarzebski.pl + (c) 2014 by Korneliusz Jarzebski +*/ + +#include +#include + +MPU6050 mpu; + +void setup() +{ + Serial.begin(115200); + + Serial.println("Initialize MPU6050"); + + while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) + { + Serial.println("Could not find a valid MPU6050 sensor, check wiring!"); + delay(500); + } +} + +void loop() +{ + // Read normalized values + Vector normAccel = mpu.readNormalizeAccel(); + + // Calculate Pitch & Roll + int pitch = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis*normAccel.YAxis + normAccel.ZAxis*normAccel.ZAxis))*180.0)/M_PI; + int roll = (atan2(normAccel.YAxis, normAccel.ZAxis)*180.0)/M_PI; + + // Output + Serial.print(" Pitch = "); + Serial.print(pitch); + Serial.print(" Roll = "); + Serial.print(roll); + + Serial.println(); + + delay(10); +} + + diff --git a/Firmware/MPU6050/examples/MPU6050_accel_simple/MPU6050_accel_simple.ino b/Firmware/MPU6050/examples/MPU6050_accel_simple/MPU6050_accel_simple.ino new file mode 100644 index 0000000..d327167 --- /dev/null +++ b/Firmware/MPU6050/examples/MPU6050_accel_simple/MPU6050_accel_simple.ino @@ -0,0 +1,94 @@ +/* + MPU6050 Triple Axis Gyroscope & Accelerometer. Simple Accelerometer Example. + Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html + GIT: https://github.com/jarzebski/Arduino-MPU6050 + Web: http://www.jarzebski.pl + (c) 2014 by Korneliusz Jarzebski +*/ + +#include +#include + +MPU6050 mpu; + +void setup() +{ + Serial.begin(115200); + + Serial.println("Initialize MPU6050"); + + while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) + { + Serial.println("Could not find a valid MPU6050 sensor, check wiring!"); + delay(500); + } + + // If you want, you can set accelerometer offsets + // mpu.setAccelOffsetX(); + // mpu.setAccelOffsetY(); + // mpu.setAccelOffsetZ(); + + checkSettings(); +} + +void checkSettings() +{ + Serial.println(); + + Serial.print(" * Sleep Mode: "); + Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled"); + + Serial.print(" * Clock Source: "); + switch(mpu.getClockSource()) + { + case MPU6050_CLOCK_KEEP_RESET: Serial.println("Stops the clock and keeps the timing generator in reset"); break; + case MPU6050_CLOCK_EXTERNAL_19MHZ: Serial.println("PLL with external 19.2MHz reference"); break; + case MPU6050_CLOCK_EXTERNAL_32KHZ: Serial.println("PLL with external 32.768kHz reference"); break; + case MPU6050_CLOCK_PLL_ZGYRO: Serial.println("PLL with Z axis gyroscope reference"); break; + case MPU6050_CLOCK_PLL_YGYRO: Serial.println("PLL with Y axis gyroscope reference"); break; + case MPU6050_CLOCK_PLL_XGYRO: Serial.println("PLL with X axis gyroscope reference"); break; + case MPU6050_CLOCK_INTERNAL_8MHZ: Serial.println("Internal 8MHz oscillator"); break; + } + + Serial.print(" * Accelerometer: "); + switch(mpu.getRange()) + { + case MPU6050_RANGE_16G: Serial.println("+/- 16 g"); break; + case MPU6050_RANGE_8G: Serial.println("+/- 8 g"); break; + case MPU6050_RANGE_4G: Serial.println("+/- 4 g"); break; + case MPU6050_RANGE_2G: Serial.println("+/- 2 g"); break; + } + + Serial.print(" * Accelerometer offsets: "); + Serial.print(mpu.getAccelOffsetX()); + Serial.print(" / "); + Serial.print(mpu.getAccelOffsetY()); + Serial.print(" / "); + Serial.println(mpu.getAccelOffsetZ()); + + Serial.println(); +} + +void loop() +{ + Vector rawAccel = mpu.readRawAccel(); + Vector normAccel = mpu.readNormalizeAccel(); + + Serial.print(" Xraw = "); + Serial.print(rawAccel.XAxis); + Serial.print(" Yraw = "); + Serial.print(rawAccel.YAxis); + Serial.print(" Zraw = "); + + Serial.println(rawAccel.ZAxis); + Serial.print(" Xnorm = "); + Serial.print(normAccel.XAxis); + Serial.print(" Ynorm = "); + Serial.print(normAccel.YAxis); + Serial.print(" Znorm = "); + Serial.println(normAccel.ZAxis); + + delay(10); +} + + diff --git a/Firmware/MPU6050/examples/MPU6050_free_fall/MPU6050_free_fall.ino b/Firmware/MPU6050/examples/MPU6050_free_fall/MPU6050_free_fall.ino new file mode 100644 index 0000000..d2d9269 --- /dev/null +++ b/Firmware/MPU6050/examples/MPU6050_free_fall/MPU6050_free_fall.ino @@ -0,0 +1,143 @@ +/* + MPU6050 Triple Axis Gyroscope & Accelerometer. Free fall detection. + Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html + GIT: https://github.com/jarzebski/Arduino-MPU6050 + Web: http://www.jarzebski.pl + (c) 2014 by Korneliusz Jarzebski +*/ + +#include +#include + +MPU6050 mpu; + +boolean ledState = false; +boolean freefallDetected = false; +int freefallBlinkCount = 0; + +void setup() +{ + Serial.begin(115200); + + Serial.println("Initialize MPU6050"); + + while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_16G)) + { + Serial.println("Could not find a valid MPU6050 sensor, check wiring!"); + delay(500); + } + + mpu.setAccelPowerOnDelay(MPU6050_DELAY_3MS); + + mpu.setIntFreeFallEnabled(true); + mpu.setIntZeroMotionEnabled(false); + mpu.setIntMotionEnabled(false); + + mpu.setDHPFMode(MPU6050_DHPF_5HZ); + + mpu.setFreeFallDetectionThreshold(17); + mpu.setFreeFallDetectionDuration(2); + + checkSettings(); + + pinMode(4, OUTPUT); + digitalWrite(4, LOW); + + attachInterrupt(0, doInt, RISING); +} + +void doInt() +{ + freefallBlinkCount = 0; + freefallDetected = true; +} + +void checkSettings() +{ + Serial.println(); + + Serial.print(" * Sleep Mode: "); + Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled"); + + Serial.print(" * Motion Interrupt: "); + Serial.println(mpu.getIntMotionEnabled() ? "Enabled" : "Disabled"); + + Serial.print(" * Zero Motion Interrupt: "); + Serial.println(mpu.getIntZeroMotionEnabled() ? "Enabled" : "Disabled"); + + Serial.print(" * Free Fall Interrupt: "); + Serial.println(mpu.getIntFreeFallEnabled() ? "Enabled" : "Disabled"); + + Serial.print(" * Free Fal Threshold: "); + Serial.println(mpu.getFreeFallDetectionThreshold()); + + Serial.print(" * Free FallDuration: "); + Serial.println(mpu.getFreeFallDetectionDuration()); + + Serial.print(" * Clock Source: "); + switch(mpu.getClockSource()) + { + case MPU6050_CLOCK_KEEP_RESET: Serial.println("Stops the clock and keeps the timing generator in reset"); break; + case MPU6050_CLOCK_EXTERNAL_19MHZ: Serial.println("PLL with external 19.2MHz reference"); break; + case MPU6050_CLOCK_EXTERNAL_32KHZ: Serial.println("PLL with external 32.768kHz reference"); break; + case MPU6050_CLOCK_PLL_ZGYRO: Serial.println("PLL with Z axis gyroscope reference"); break; + case MPU6050_CLOCK_PLL_YGYRO: Serial.println("PLL with Y axis gyroscope reference"); break; + case MPU6050_CLOCK_PLL_XGYRO: Serial.println("PLL with X axis gyroscope reference"); break; + case MPU6050_CLOCK_INTERNAL_8MHZ: Serial.println("Internal 8MHz oscillator"); break; + } + + Serial.print(" * Accelerometer: "); + switch(mpu.getRange()) + { + case MPU6050_RANGE_16G: Serial.println("+/- 16 g"); break; + case MPU6050_RANGE_8G: Serial.println("+/- 8 g"); break; + case MPU6050_RANGE_4G: Serial.println("+/- 4 g"); break; + case MPU6050_RANGE_2G: Serial.println("+/- 2 g"); break; + } + + Serial.print(" * Accelerometer offsets: "); + Serial.print(mpu.getAccelOffsetX()); + Serial.print(" / "); + Serial.print(mpu.getAccelOffsetY()); + Serial.print(" / "); + Serial.println(mpu.getAccelOffsetZ()); + + Serial.print(" * Accelerometer power delay: "); + switch(mpu.getAccelPowerOnDelay()) + { + case MPU6050_DELAY_3MS: Serial.println("3ms"); break; + case MPU6050_DELAY_2MS: Serial.println("2ms"); break; + case MPU6050_DELAY_1MS: Serial.println("1ms"); break; + case MPU6050_NO_DELAY: Serial.println("0ms"); break; + } + + Serial.println(); +} + +void loop() +{ + Vector rawAccel = mpu.readRawAccel(); + Activites act = mpu.readActivites(); + + Serial.print(act.isFreeFall); + Serial.print("\n"); + + if (freefallDetected) + { + ledState = !ledState; + + digitalWrite(4, ledState); + + freefallBlinkCount++; + + if (freefallBlinkCount == 20) + { + freefallDetected = false; + ledState = false; + digitalWrite(4, ledState); + } + } + + delay(100); + +} diff --git a/Firmware/MPU6050/examples/MPU6050_gyro_pitch_roll_yaw/MPU6050_gyro_pitch_roll_yaw.ino b/Firmware/MPU6050/examples/MPU6050_gyro_pitch_roll_yaw/MPU6050_gyro_pitch_roll_yaw.ino new file mode 100644 index 0000000..c854158 --- /dev/null +++ b/Firmware/MPU6050/examples/MPU6050_gyro_pitch_roll_yaw/MPU6050_gyro_pitch_roll_yaw.ino @@ -0,0 +1,65 @@ +/* + MPU6050 Triple Axis Gyroscope & Accelerometer. Pitch & Roll & Yaw Gyroscope Example. + Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html + GIT: https://github.com/jarzebski/Arduino-MPU6050 + Web: http://www.jarzebski.pl + (c) 2014 by Korneliusz Jarzebski +*/ + +#include +#include + +MPU6050 mpu; + +// Timers +unsigned long timer = 0; +float timeStep = 0.01; + +// Pitch, Roll and Yaw values +float pitch = 0; +float roll = 0; +float yaw = 0; + +void setup() +{ + Serial.begin(115200); + + // Initialize MPU6050 + while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) + { + Serial.println("Could not find a valid MPU6050 sensor, check wiring!"); + delay(500); + } + + // Calibrate gyroscope. The calibration must be at rest. + // If you don't want calibrate, comment this line. + mpu.calibrateGyro(); + + // Set threshold sensivty. Default 3. + // If you don't want use threshold, comment this line or set 0. + mpu.setThreshold(3); +} + +void loop() +{ + timer = millis(); + + // Read normalized values + Vector norm = mpu.readNormalizeGyro(); + + // Calculate Pitch, Roll and Yaw + pitch = pitch + norm.YAxis * timeStep; + roll = roll + norm.XAxis * timeStep; + yaw = yaw + norm.ZAxis * timeStep; + + // Output raw + Serial.print(" Pitch = "); + Serial.print(pitch); + Serial.print(" Roll = "); + Serial.print(roll); + Serial.print(" Yaw = "); + Serial.println(yaw); + + // Wait to full timeStep period + delay((timeStep*1000) - (millis() - timer)); +} diff --git a/Firmware/MPU6050/examples/MPU6050_gyro_simple/MPU6050_gyro_simple.ino b/Firmware/MPU6050/examples/MPU6050_gyro_simple/MPU6050_gyro_simple.ino new file mode 100644 index 0000000..b0ff8d6 --- /dev/null +++ b/Firmware/MPU6050/examples/MPU6050_gyro_simple/MPU6050_gyro_simple.ino @@ -0,0 +1,103 @@ +/* + MPU6050 Triple Axis Gyroscope & Accelerometer. Simple Gyroscope Example. + Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html + GIT: https://github.com/jarzebski/Arduino-MPU6050 + Web: http://www.jarzebski.pl + (c) 2014 by Korneliusz Jarzebski +*/ + +#include +#include + +MPU6050 mpu; + +void setup() +{ + Serial.begin(115200); + + // Initialize MPU6050 + Serial.println("Initialize MPU6050"); + while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) + { + Serial.println("Could not find a valid MPU6050 sensor, check wiring!"); + delay(500); + } + + // If you want, you can set gyroscope offsets + // mpu.setGyroOffsetX(155); + // mpu.setGyroOffsetY(15); + // mpu.setGyroOffsetZ(15); + + // Calibrate gyroscope. The calibration must be at rest. + // If you don't want calibrate, comment this line. + mpu.calibrateGyro(); + + // Set threshold sensivty. Default 3. + // If you don't want use threshold, comment this line or set 0. + mpu.setThreshold(3); + + // Check settings + checkSettings(); +} + +void checkSettings() +{ + Serial.println(); + + Serial.print(" * Sleep Mode: "); + Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled"); + + Serial.print(" * Clock Source: "); + switch(mpu.getClockSource()) + { + case MPU6050_CLOCK_KEEP_RESET: Serial.println("Stops the clock and keeps the timing generator in reset"); break; + case MPU6050_CLOCK_EXTERNAL_19MHZ: Serial.println("PLL with external 19.2MHz reference"); break; + case MPU6050_CLOCK_EXTERNAL_32KHZ: Serial.println("PLL with external 32.768kHz reference"); break; + case MPU6050_CLOCK_PLL_ZGYRO: Serial.println("PLL with Z axis gyroscope reference"); break; + case MPU6050_CLOCK_PLL_YGYRO: Serial.println("PLL with Y axis gyroscope reference"); break; + case MPU6050_CLOCK_PLL_XGYRO: Serial.println("PLL with X axis gyroscope reference"); break; + case MPU6050_CLOCK_INTERNAL_8MHZ: Serial.println("Internal 8MHz oscillator"); break; + } + + Serial.print(" * Gyroscope: "); + switch(mpu.getScale()) + { + case MPU6050_SCALE_2000DPS: Serial.println("2000 dps"); break; + case MPU6050_SCALE_1000DPS: Serial.println("1000 dps"); break; + case MPU6050_SCALE_500DPS: Serial.println("500 dps"); break; + case MPU6050_SCALE_250DPS: Serial.println("250 dps"); break; + } + + Serial.print(" * Gyroscope offsets: "); + Serial.print(mpu.getGyroOffsetX()); + Serial.print(" / "); + Serial.print(mpu.getGyroOffsetY()); + Serial.print(" / "); + Serial.println(mpu.getGyroOffsetZ()); + + Serial.println(); +} + +void loop() +{ + Vector rawGyro = mpu.readRawGyro(); + Vector normGyro = mpu.readNormalizeGyro(); + + Serial.print(" Xraw = "); + Serial.print(rawGyro.XAxis); + Serial.print(" Yraw = "); + Serial.print(rawGyro.YAxis); + Serial.print(" Zraw = "); + Serial.println(rawGyro.ZAxis); + + Serial.print(" Xnorm = "); + Serial.print(normGyro.XAxis); + Serial.print(" Ynorm = "); + Serial.print(normGyro.YAxis); + Serial.print(" Znorm = "); + Serial.println(normGyro.ZAxis); + + delay(10); +} + + diff --git a/Firmware/MPU6050/examples/MPU6050_motion/MPU6050_motion.ino b/Firmware/MPU6050/examples/MPU6050_motion/MPU6050_motion.ino new file mode 100644 index 0000000..27c7c29 --- /dev/null +++ b/Firmware/MPU6050/examples/MPU6050_motion/MPU6050_motion.ino @@ -0,0 +1,156 @@ +/* + MPU6050 Triple Axis Gyroscope & Accelerometer. Motion detection. + Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html + GIT: https://github.com/jarzebski/Arduino-MPU6050 + Web: http://www.jarzebski.pl + (c) 2014 by Korneliusz Jarzebski +*/ + +#include +#include + +MPU6050 mpu; + +void setup() +{ + Serial.begin(115200); + + Serial.println("Initialize MPU6050"); + + while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_16G)) + { + Serial.println("Could not find a valid MPU6050 sensor, check wiring!"); + delay(500); + } + + mpu.setAccelPowerOnDelay(MPU6050_DELAY_3MS); + + mpu.setIntFreeFallEnabled(false); + mpu.setIntZeroMotionEnabled(false); + mpu.setIntMotionEnabled(false); + + mpu.setDHPFMode(MPU6050_DHPF_5HZ); + + mpu.setMotionDetectionThreshold(2); + mpu.setMotionDetectionDuration(5); + + mpu.setZeroMotionDetectionThreshold(4); + mpu.setZeroMotionDetectionDuration(2); + + checkSettings(); + + pinMode(4, OUTPUT); + digitalWrite(4, LOW); + + pinMode(7, OUTPUT); + digitalWrite(7, LOW); +} + +void checkSettings() +{ + Serial.println(); + + Serial.print(" * Sleep Mode: "); + Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled"); + + Serial.print(" * Motion Interrupt: "); + Serial.println(mpu.getIntMotionEnabled() ? "Enabled" : "Disabled"); + + Serial.print(" * Zero Motion Interrupt: "); + Serial.println(mpu.getIntZeroMotionEnabled() ? "Enabled" : "Disabled"); + + Serial.print(" * Free Fall Interrupt: "); + Serial.println(mpu.getIntFreeFallEnabled() ? "Enabled" : "Disabled"); + + Serial.print(" * Motion Threshold: "); + Serial.println(mpu.getMotionDetectionThreshold()); + + Serial.print(" * Motion Duration: "); + Serial.println(mpu.getMotionDetectionDuration()); + + Serial.print(" * Zero Motion Threshold: "); + Serial.println(mpu.getZeroMotionDetectionThreshold()); + + Serial.print(" * Zero Motion Duration: "); + Serial.println(mpu.getZeroMotionDetectionDuration()); + + Serial.print(" * Clock Source: "); + switch(mpu.getClockSource()) + { + case MPU6050_CLOCK_KEEP_RESET: Serial.println("Stops the clock and keeps the timing generator in reset"); break; + case MPU6050_CLOCK_EXTERNAL_19MHZ: Serial.println("PLL with external 19.2MHz reference"); break; + case MPU6050_CLOCK_EXTERNAL_32KHZ: Serial.println("PLL with external 32.768kHz reference"); break; + case MPU6050_CLOCK_PLL_ZGYRO: Serial.println("PLL with Z axis gyroscope reference"); break; + case MPU6050_CLOCK_PLL_YGYRO: Serial.println("PLL with Y axis gyroscope reference"); break; + case MPU6050_CLOCK_PLL_XGYRO: Serial.println("PLL with X axis gyroscope reference"); break; + case MPU6050_CLOCK_INTERNAL_8MHZ: Serial.println("Internal 8MHz oscillator"); break; + } + + Serial.print(" * Accelerometer: "); + switch(mpu.getRange()) + { + case MPU6050_RANGE_16G: Serial.println("+/- 16 g"); break; + case MPU6050_RANGE_8G: Serial.println("+/- 8 g"); break; + case MPU6050_RANGE_4G: Serial.println("+/- 4 g"); break; + case MPU6050_RANGE_2G: Serial.println("+/- 2 g"); break; + } + + Serial.print(" * Accelerometer offsets: "); + Serial.print(mpu.getAccelOffsetX()); + Serial.print(" / "); + Serial.print(mpu.getAccelOffsetY()); + Serial.print(" / "); + Serial.println(mpu.getAccelOffsetZ()); + + Serial.print(" * Accelerometer power delay: "); + switch(mpu.getAccelPowerOnDelay()) + { + case MPU6050_DELAY_3MS: Serial.println("3ms"); break; + case MPU6050_DELAY_2MS: Serial.println("2ms"); break; + case MPU6050_DELAY_1MS: Serial.println("1ms"); break; + case MPU6050_NO_DELAY: Serial.println("0ms"); break; + } + + Serial.println(); +} + +void loop() +{ + Vector rawAccel = mpu.readRawAccel(); + Activites act = mpu.readActivites(); + + if (act.isActivity) + { + digitalWrite(4, HIGH); + } else + { + digitalWrite(4, LOW); + } + + if (act.isInactivity) + { + digitalWrite(7, HIGH); + } else + { + digitalWrite(7, LOW); + } + + Serial.print(act.isActivity); + Serial.print(act.isInactivity); + + Serial.print(" "); + Serial.print(act.isPosActivityOnX); + Serial.print(act.isNegActivityOnX); + Serial.print(" "); + + Serial.print(act.isPosActivityOnY); + Serial.print(act.isNegActivityOnY); + Serial.print(" "); + + Serial.print(act.isPosActivityOnZ); + Serial.print(act.isNegActivityOnZ); + Serial.print("\n"); + delay(50); +} + + diff --git a/Firmware/MPU6050/examples/MPU6050_temperature/MPU6050_temperature.ino b/Firmware/MPU6050/examples/MPU6050_temperature/MPU6050_temperature.ino new file mode 100644 index 0000000..d2d5958 --- /dev/null +++ b/Firmware/MPU6050/examples/MPU6050_temperature/MPU6050_temperature.ino @@ -0,0 +1,38 @@ +/* + MPU6050 Triple Axis Gyroscope & Accelerometer. Temperature Example. + Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html + GIT: https://github.com/jarzebski/Arduino-MPU6050 + Web: http://www.jarzebski.pl + (c) 2014 by Korneliusz Jarzebski +*/ + +#include +#include + +MPU6050 mpu; + +void setup() +{ + Serial.begin(115200); + + Serial.println("Initialize MPU6050"); + + while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) + { + Serial.println("Could not find a valid MPU6050 sensor, check wiring!"); + delay(500); + } +} + +void loop() +{ + float temp = mpu.readTemperature(); + + Serial.print(" Temp = "); + Serial.print(temp); + Serial.println(" *C"); + + delay(500); +} + + diff --git a/master.ino b/Firmware/master.ino similarity index 94% rename from master.ino rename to Firmware/master.ino index 809051f..6b9ae6f 100644 --- a/master.ino +++ b/Firmware/master.ino @@ -1,169 +1,169 @@ -/* - This is the source code for the master-module of Star Track. - Required Libraries: - https://virtuabotix-virtuabotixllc.netdna-ssl.com/core/wp-content/uploads/2014/01/virtuabotixRTC.zip - https://drive.google.com/folderview?id=0B1p6T9dV6tnqSjU2WnBXREZPbms&usp=sharing - - Created 20 July 2016 by Görkem Bozkurt - */ -#include -#include -#include -MPU6050 mpu; -//define RTC. -virtuabotixRTC myRTC(A0, A1, A2); -double M,Y,D,MN,H,S; -double A,B; -double location =32.88;//your longtitude. -double location2 =39.933363//your latitude. -double LST_degrees;//variable to store local side real time(LST) in degrees. -double LST_hours;//variable to store local side real time(LST) in decimal hours. -unsigned long timer = 0; -float timeStep = 0.01; -// Pitch and Yaw values -double pitch = 0; -double yaw = 0; -double val = 0;//variable to store the user input DEC -double val2 = 0;//variable to store the user input RA -double temp = val2;//temporary value to store val2 -const int stahp=7,stahp2=10; -const int cw=8,cw2=11; -const int ccw=6,ccw2=9; -void setup() { - //set date-time according to (seconds, minutes, hours, day of the week, day of the month, month, year) - myRTC.setDS1302Time(00, 38, 23, 5, 27, 7, 2016); - Serial.begin(115200); - pinMode(stahp,OUTPUT); - pinMode(cw,OUTPUT); - pinMode(ccw,OUTPUT); - pinMode(stahp2,OUTPUT); - pinMode(cw2,OUTPUT); - pinMode(ccw2,OUTPUT); - delay(5000);//wait before starting - while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) - { - } - mpu.calibrateGyro(); - mpu.setThreshold(3); -}//--(end setup )--- -void loop() -{ - //this will update the RA degrees with sidereal time 1degree at a time - //this way the object or star on the sky is tracked. - if(location2>0){//for the northern hemisphere. - if( floor(LST_degrees)==LST_degrees ){ - if (LST_degrees>180){ - val2 = temp+(360-LST_degrees); - }else{ - val2 = temp-LST_degrees; //use val2 = temp+LST_degrees; if you are located in the southern hemisphere. - } - } - }else{//for the southern hemisphere. - if( floor(LST_degrees)==LST_degrees ){ - if (LST_degrees>180){ - val2 = temp-(360-LST_degrees); - }else{ - val2 = temp+LST_degrees; //use val2 = temp+LST_degrees; if you are located in the southern hemisphere. - } - } - } - myRTC.updateTime(); - LST_time(); - recvdata(); - pitch_check(); - yaw_check(); - timer = millis(); - Vector norm = mpu.readNormalizeGyro(); - //I've put the sensor with a 90 degree angle on the setup due to - //cable connection problems. Because of that the data values from the mpu6050 chip are - //different in this case: - //roll data(X-axis) is pitch. - //pitch data(Y-axis) is yaw. - yaw = yaw + norm.YAxis * timeStep; - pitch = pitch + norm.XAxis * timeStep; - Serial.print(" Yaw = "); - Serial.print(yaw); - Serial.print(" Pitch = "); - Serial.print(pitch); - Serial.print(" LST_d = "); - Serial.print(LST_degrees); - Serial.print(" LST_h = "); - Serial.println(LST_hours);//local sidereal time in decimal hours. - delay((timeStep*1000) - (millis() - timer));//timer for the gyro. -} -void recvdata(){ - //This function receives data from serial as (0.00,0.00) - //splits it to strings by the comma "," - //than converts them to doubles - if (Serial.available() > 0){ - String a= Serial.readString(); - String value1, value2; - // For loop which will separate the String in parts - // and assign them the the variables we declare - for (int i = 0; i < a.length(); i++) { - if (a.substring(i, i+1) == ",") { - value2 = a.substring(0, i); - value1= a.substring(i+1); - break; - } - } - val=90-value1.toFloat(); - val2=value2.toFloat(); - temp = val2; - } -} -void pitch_check(){ - //check if pitch is high, low or equal to the user input - //send commands to slave-module to start and stop motors - if(floor(pitch*100)/100==floor(val*100)/100){ - digitalWrite(stahp,HIGH); - }else{ - digitalWrite(stahp,LOW); - } - if(floor(pitch*100)floor(val*100)){ - digitalWrite(ccw,HIGH); - }else{ - digitalWrite(ccw,LOW); - } -} -void yaw_check(){ - //check if yaw is high, low or equal to the user input - //send commands to slave-module to start and stop motors - if(floor(yaw*100)==floor(val2*100)){ - digitalWrite(stahp2,HIGH); - }else{ - digitalWrite(stahp2,LOW); - } - if(floor(yaw*100)floor(val2*100)){ - digitalWrite(ccw2,HIGH); - }else{ - digitalWrite(ccw2,LOW); - } -} -void LST_time(){ - //Calculates local sidereal time based on this calculation, - //http://www.stargazing.net/kepler/altaz.html - M = (double) myRTC.month; - Y = (double) myRTC.year; - D = (double) myRTC.dayofmonth; - MN = (double) myRTC.minutes; - H = (double) myRTC.hours; - S = (double) myRTC.seconds; - A = (double)(Y-2000)*365.242199; - B = (double)(M-1)*30.4368499; - double JDN2000=A+B+(D-1)+myRTC.hours/24; - double decimal_time = H+(MN/60)+(S/3600) ; - double LST = 100.46 + 0.985647 * JDN2000 + location + 15*decimal_time; - LST_degrees = (LST-(floor(LST/360)*360)); - LST_hours = LST_degrees/15; -} +/* + This is the source code for the master-module of Star Track. + Required Libraries: + https://virtuabotix-virtuabotixllc.netdna-ssl.com/core/wp-content/uploads/2014/01/virtuabotixRTC.zip + https://drive.google.com/folderview?id=0B1p6T9dV6tnqSjU2WnBXREZPbms&usp=sharing + + Created 20 July 2016 by Görkem Bozkurt + */ +#include +#include +#include +MPU6050 mpu; +//define RTC. +virtuabotixRTC myRTC(A0, A1, A2); +double M,Y,D,MN,H,S; +double A,B; +double location =32.88;//your longtitude. +double location2 =39.933363//your latitude. +double LST_degrees;//variable to store local side real time(LST) in degrees. +double LST_hours;//variable to store local side real time(LST) in decimal hours. +unsigned long timer = 0; +float timeStep = 0.01; +// Pitch and Yaw values +double pitch = 0; +double yaw = 0; +double val = 0;//variable to store the user input DEC +double val2 = 0;//variable to store the user input RA +double temp = val2;//temporary value to store val2 +const int stahp=7,stahp2=10; +const int cw=8,cw2=11; +const int ccw=6,ccw2=9; +void setup() { + //set date-time according to (seconds, minutes, hours, day of the week, day of the month, month, year) + myRTC.setDS1302Time(00, 38, 23, 5, 27, 7, 2016); + Serial.begin(115200); + pinMode(stahp,OUTPUT); + pinMode(cw,OUTPUT); + pinMode(ccw,OUTPUT); + pinMode(stahp2,OUTPUT); + pinMode(cw2,OUTPUT); + pinMode(ccw2,OUTPUT); + delay(5000);//wait before starting + while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) + { + } + mpu.calibrateGyro(); + mpu.setThreshold(3); +}//--(end setup )--- +void loop() +{ + //this will update the RA degrees with sidereal time 1degree at a time + //this way the object or star on the sky is tracked. + if(location2>0){//for the northern hemisphere. + if( floor(LST_degrees)==LST_degrees ){ + if (LST_degrees>180){ + val2 = temp+(360-LST_degrees); + }else{ + val2 = temp-LST_degrees; //use val2 = temp+LST_degrees; if you are located in the southern hemisphere. + } + } + }else{//for the southern hemisphere. + if( floor(LST_degrees)==LST_degrees ){ + if (LST_degrees>180){ + val2 = temp-(360-LST_degrees); + }else{ + val2 = temp+LST_degrees; //use val2 = temp+LST_degrees; if you are located in the southern hemisphere. + } + } + } + myRTC.updateTime(); + LST_time(); + recvdata(); + pitch_check(); + yaw_check(); + timer = millis(); + Vector norm = mpu.readNormalizeGyro(); + //I've put the sensor with a 90 degree angle on the setup due to + //cable connection problems. Because of that the data values from the mpu6050 chip are + //different in this case: + //roll data(X-axis) is pitch. + //pitch data(Y-axis) is yaw. + yaw = yaw + norm.YAxis * timeStep; + pitch = pitch + norm.XAxis * timeStep; + Serial.print(" Yaw = "); + Serial.print(yaw); + Serial.print(" Pitch = "); + Serial.print(pitch); + Serial.print(" LST_d = "); + Serial.print(LST_degrees); + Serial.print(" LST_h = "); + Serial.println(LST_hours);//local sidereal time in decimal hours. + delay((timeStep*1000) - (millis() - timer));//timer for the gyro. +} +void recvdata(){ + //This function receives data from serial as (0.00,0.00) + //splits it to strings by the comma "," + //than converts them to doubles + if (Serial.available() > 0){ + String a= Serial.readString(); + String value1, value2; + // For loop which will separate the String in parts + // and assign them the the variables we declare + for (int i = 0; i < a.length(); i++) { + if (a.substring(i, i+1) == ",") { + value2 = a.substring(0, i); + value1= a.substring(i+1); + break; + } + } + val=90-value1.toFloat(); + val2=value2.toFloat(); + temp = val2; + } +} +void pitch_check(){ + //check if pitch is high, low or equal to the user input + //send commands to puppet-module to start and stop motors + if(floor(pitch*100)/100==floor(val*100)/100){ + digitalWrite(stahp,HIGH); + }else{ + digitalWrite(stahp,LOW); + } + if(floor(pitch*100)floor(val*100)){ + digitalWrite(ccw,HIGH); + }else{ + digitalWrite(ccw,LOW); + } +} +void yaw_check(){ + //check if yaw is high, low or equal to the user input + //send commands to puppet-module to start and stop motors + if(floor(yaw*100)==floor(val2*100)){ + digitalWrite(stahp2,HIGH); + }else{ + digitalWrite(stahp2,LOW); + } + if(floor(yaw*100)floor(val2*100)){ + digitalWrite(ccw2,HIGH); + }else{ + digitalWrite(ccw2,LOW); + } +} +void LST_time(){ + //Calculates local sidereal time based on this calculation, + //http://www.stargazing.net/kepler/altaz.html + M = (double) myRTC.month; + Y = (double) myRTC.year; + D = (double) myRTC.dayofmonth; + MN = (double) myRTC.minutes; + H = (double) myRTC.hours; + S = (double) myRTC.seconds; + A = (double)(Y-2000)*365.242199; + B = (double)(M-1)*30.4368499; + double JDN2000=A+B+(D-1)+myRTC.hours/24; + double decimal_time = H+(MN/60)+(S/3600) ; + double LST = 100.46 + 0.985647 * JDN2000 + location + 15*decimal_time; + LST_degrees = (LST-(floor(LST/360)*360)); + LST_hours = LST_degrees/15; +} diff --git a/slave.ino b/Firmware/puppet.ino similarity index 97% rename from slave.ino rename to Firmware/puppet.ino index f623c08..1b53330 100644 --- a/slave.ino +++ b/Firmware/puppet.ino @@ -1,5 +1,5 @@ /* -this is the source code for the slave module of Star Track +this is the source code for the puppet module of Star Track Required Libraries: http://www.airspayce.com/mikem/arduino/AccelStepper/AccelStepper-1.51.zip Created 20 July 2016 by Görkem Bozkurt @@ -75,4 +75,4 @@ void motor_pitch(){ } } -} +} diff --git a/README.md b/README.md index b35de69..0b3a7f1 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,9 @@ # Star-Track Arduino powered GoTo-mount inspired star tracking system. -more info: http://www.instructables.com/id/Star-Track-Arduino-Powered-Star-Pointer-and-Tracke/ +more info: https://gorkem.cc/projects/StarTrack/ -There are two Arduino's working in order. Arduino Uno being master and nano being the slave. +There are two Arduino's working in order. Arduino Uno being master and nano being the puppet. Master module @@ -27,7 +27,7 @@ and 6 outputs, 6-stahp2(DEC motor stop) -The RTC is set to UTC time, a function calculates local sidereal time in degrees and rotates the mount to 0h RA position. The loop constantly checks if the gyro data is equal to the user input & sidereal time data. The default is 0,0. If there is a change and the equality breaks the master module send a command to the slave module. +The RTC is set to UTC time, a function calculates local sidereal time in degrees and rotates the mount to 0h RA position. The loop constantly checks if the gyro data is equal to the user input & sidereal time data. The default is 0,0. If there is a change and the equality breaks the master module send a command to the puppet module. If the user value is higher than the gyro value a signal is sent through cw the motor turns clockwise.