TiltyIMU::TiltyIMU()
{
// Instantiate sensors
imu = MPU6050();
magn = HMC5883();
alt = MPL3115A2();
}
void GetHMC5883LRawMeasure(uint8_t* buffer) {
MPU6050();
HMC5883L();
HMC5883L_SetScale(13);
HMC5883L_SetMeasurementMode(Measurement_Continuous);
IICReadNBytes(HMC5883L_Address,DataRegisterBegin,6,buffer);
}
#include "application.h"
#include "MPU6050.h"
#include "Adafruit_NeoPixel.h"
#include <math.h>
#include "UDPLogger.h"
#define PIXEL_COUNT 16
#define LED_MODULO 4
Adafruit_NeoPixel strip1 = Adafruit_NeoPixel(PIXEL_COUNT, D2, WS2812B);
Adafruit_NeoPixel strip2 = Adafruit_NeoPixel(PIXEL_COUNT, D3, WS2812B);
MPU6050 mpu = MPU6050(MPU6050::Address0);
UDPLogger logger;
static bool ballTurnedOn = false;
uint32_t Wheel(byte WheelPos);
void rainbow(uint8_t modulo, uint8_t wait);
void ball(void);
void blinkLED(unsigned int count, unsigned int mydelay)
{
int led = D7;
pinMode(led, OUTPUT);
while (count > 0) {
digitalWrite(led, HIGH);
delay(mydelay); // Wait for 1000mS = 1 second
digitalWrite(led, LOW);
delay(mydelay); // Wait for 1 second in off mode
FreeIMU::FreeIMU() {
#if HAS_ADXL345()
acc = ADXL345();
#elif HAS_BMA180()
acc = BMA180();
#endif
#if HAS_HMC5883L()
magn = HMC58X3();
#endif
#if HAS_ITG3200()
gyro = ITG3200();
#elif HAS_MPU6050()
accgyro = MPU6050(); // I2C
#elif HAS_MPU6000()
accgyro = MPU6050(); // SPI for Arduimu v3
#endif
#if HAS_MS5611()
baro = MS561101BA();
#endif
// initialize quaternion
q0 = 1.0f;
q1 = 0.0f;
q2 = 0.0f;
q3 = 0.0f;
exInt = 0.0;
eyInt = 0.0;
ezInt = 0.0;
twoKp = twoKpDef;
twoKi = twoKiDef;
integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;
lastUpdate = 0;
now = 0;
#ifndef CALIBRATION_H
// initialize scale factors to neutral values
acc_scale_x = 1;
acc_scale_y = 1;
acc_scale_z = 1;
magn_scale_x = 1;
magn_scale_y = 1;
magn_scale_z = 1;
#else
// get values from global variables of same name defined in calibration.h
acc_off_x = ::acc_off_x;
acc_off_y = ::acc_off_y;
acc_off_z = ::acc_off_z;
acc_scale_x = ::acc_scale_x;
acc_scale_y = ::acc_scale_y;
acc_scale_z = ::acc_scale_z;
magn_off_x = ::magn_off_x;
magn_off_y = ::magn_off_y;
magn_off_z = ::magn_off_z;
magn_scale_x = ::magn_scale_x;
magn_scale_y = ::magn_scale_y;
magn_scale_z = ::magn_scale_z;
#endif
}
void FreeIMU::init(int acc_addr, int gyro_addr, bool fastmode) {
#else
void FreeIMU::init(int accgyro_addr, bool fastmode) {
#endif
delay(5);
// disable internal pullups of the ATMEGA which Wire enable by default
#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
// deactivate internal pull-ups for twi
// as per note from atmega8 manual pg167
cbi(PORTC, 4);
cbi(PORTC, 5);
#elif defined(__AVR__)
// deactivate internal pull-ups for twi
// as per note from atmega128 manual pg204
cbi(PORTD, 0);
cbi(PORTD, 1);
#endif
#ifndef I2C_T3_H
#if defined(__AVR__) // only valid on AVR, not on 32bit platforms (eg: Arduino 2, Teensy 3.0)
if(fastmode) { // switch to 400KHz I2C - eheheh
TWBR = ((F_CPU / 400000L) - 16) / 2; // see twi_init in Wire/utility/twi.c
}
#elif defined(__arm__)
if(fastmode) {
#if defined(CORE_TEENSY) && F_BUS == 48000000 && !defined(I2C_T3_H)
I2C0_F = 0x1A; // Teensy 3.0 at 48 or 96 MHz
I2C0_FLT = 2;
#elif defined(CORE_TEENSY) && F_BUS == 24000000
I2C0_F = 0x45; // Teensy 3.0 at 24 MHz
I2C0_FLT = 1;
#endif
}
#endif
#endif
#if HAS_ADXL345()
// init ADXL345
acc.init(acc_addr);
#elif HAS_BMA180()
// init BMA180
acc.setAddress(acc_addr);
acc.SoftReset();
acc.enableWrite();
acc.SetFilter(acc.F10HZ);
acc.setGSensitivty(acc.G15);
acc.SetSMPSkip();
acc.SetISRMode();
acc.disableWrite();
#endif
#if HAS_ITG3200()
// init ITG3200
gyro.init(gyro_addr);
delay(1000);
// calibrate the ITG3200
gyro.zeroCalibrate(128,5);
#endif
#if HAS_MPU6050()
accgyro = MPU6050(accgyro_addr);
accgyro.initialize();
accgyro.setI2CMasterModeEnabled(0);
accgyro.setI2CBypassEnabled(1);
accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
delay(5);
#endif
#if HAS_MPU6000()
accgyro = MPU6050(true, accgyro_addr);
accgyro.initialize();
accgyro.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
delay(5);
#endif
#if HAS_HMC5883L()
// init HMC5843
magn.init(false); // Don't set mode yet, we'll do that later on.
// Calibrate HMC using self test, not recommended to change the gain after calibration.
magn.calibrate(1); // Use gain 1=default, valid 0-7, 7 not recommended.
// Single mode conversion was used in calibration, now set continuous mode
magn.setMode(0);
delay(10);
magn.setDOR(B110);
#endif
#if HAS_MS5611()
baro.init(FIMU_BARO_ADDR);
#endif
// zero gyro
zeroGyro();
#ifndef CALIBRATION_H
// load calibration from eeprom
calLoad();
#endif
}
