Zacznę od wyjaśnienia niewtajemniczonym, co ten czujnik mierzy. Akcelerometr jest to urządzenie mierzące przyspieszenia. Przyspieszenia są ściśle związane z działającymi na robota siłami. Pojawiają się, gdy prędkość robota się zmienia. Poprzez badanie przyspieszeń można sprawdzić jak szybko robot przyspiesza lub czy działają na niego jakieś zewnętrzne siły. Siłą która zawsze działa jest siła ciężkości, więc akcelerometr pozwala zmierzyć przyspieszenie ziemskie - 1g. Wiadomo, że to przyspieszenie skierowane jest do środka Ziemi, więc sensor pozwala również ustalić gdzie jest "dół", a tym samym nachylenie robota.

Magnetometr to bardzo czułe urządzenie pozwalające mierzyć pola magnetyczne. Pola te mogą pochodzić od magnesu umieszczonego w pobliżu, od pól wytworzonych przez płynący prąd lub od Ziemi. Tym samym czujnik ten pozwala między innymi ustalić gdzie jest ziemski biegun magnetyczny - północ.

Oba urządzenia zostały umieszczone w jednym chipie LSM303DLH (dokumentacja) a sklep pololu.com zapewnił wszelkie niezbędne komponenty elektroniczne wymagane do prawidłowego działania (strona produktu).

W takiej formie zakupiłem ten czujnik. Po pierwszych próbach uruchomiłem go bezproblemowo i udało mi się uzyskać pomiary. Po przeniesieniu wszystkiego do Lego spotkała mnie jednak niemiła niespodzianka. Czujnik odmawiał posłuszeństwa, a jedynym komunikatem jaki otrzymywałem był błąd komunikacji z urządzeniem. Zupełnie przypadkiem znalazłem rozwiązanie. Nie wiem dlaczego, ale okazało się, że zastąpienie zasilania Lego 4,3V napięciem 3,3V rozwiązywało sprawę. Musiałem więc wykonać "przejściówkę" do sensora.

Wykonanie takiego układu nie jest trudne. Po lewej znajduje się schemat ideowy. Zasilanie Lego 4,3V zostaje podane na wejście stabilizatora LF33CV. Na wyjściu uzyskujemy 3,3V, którym zasilany będzie czujnik. Dodane zostały zalecane przez producenta dwa kondensatory. Całość zmontowałem na płytce drukowanej. Dodałem gniazdo na przewód od Lego, oraz wejście na czujnik.

Tutaj przedstawiony jest projekt płytki drukowanej. Plik projektu programu KiCad można pobrać tutaj. Gotowy do wydruku plik XPS tutaj. Płytkę wykonałem metodą termotransferu. Na zdjęciach poniżej można zobaczyć końcowy efekt. Jeżeli chodzi o przewód połączeniowy, to zastosowałem standardowe końcówki RJ12. Aby połączyć ją z Lego musiałem jedną stronę przerobić, aby pasowała do zmodyfikowanych gniazd Lego.

W Internecie jest dużo informacji, jak przerobić takie końcówki. Ja po prostu od już zaciśniętej odpiłowałem zbędne fragmenty, a z nowej końcówki odpiłowałem potrzebne. Wszystko połączyłem klejem w odpowiedni sposób. Można powiedzieć, że jedną końcówkę zmarnowałem, ale tak było najszybciej.

Po wykonaniu układu możliwe było podłączenie czujnika. Sposób połączenia przedstawiony jest na zdjęciach poniżej. Teraz Lego poprawnie rozpoznawało cały układ i możliwy był odczyt pomiarów. Dla ułatwienia napisałem krótką bibliotekę dla tego czujnika. Dostępna jest do pobrania poniżej. Na pewno wymaga dopracowania, ale z jej użyciem udało mi się przeprowadzić testy udokumentowane na filmach. W pierwszym teście robot zachowuje ciągle kierunek północny. W drugim wykonuje precyzyjne skręty o 90 stopni.

UPDATE: Do biblioteki zostały dodane dwie dyrektywy #define _ACCMAG_PROBLEMS oraz #define _ACCMAG_DELAY 2 W przypadku błędów komunikacji, zawieszania się programu itp. należy dostosować parametr _ACCMAG_DELAY dla swoich potrzeb. U mnie wartość 2 działa dobrze. Jeżeli urządzenie działa sprawnie, można zakomentować obie te linie.

Ważną rzeczą przy używaniu tego czujnika jest jego umiejscowienie. W silnikach znajdują się magnesy, w kostce NXT jest dużo elektroniki. Wszystko to zakłóca pomiary pola magnetycznego. Czujnik musi być umieszczony z dala od tych elementów, jak pokazane zostało na filmach. Bliskość zewnętrznych źródeł pola magnetycznego zakłóca pomiar północy. Głośnik znajdujący się w odległości 2 metrów jest rejestrowany przez czujnik. Kolejną ważną uwagą jest konieczność kalibracji sensora. Kalibracja polega na zarejestrowaniu minimalnych i maksymalnych wartości pola magnetycznego i skalowanie wszystkich pomiarów. W przykładzie prezentującym dokładne skręcanie kalibracja polegała na mierzeniu wartości maksymalnych i minimalnych gdy robot obracał się dookoła własnej osi przez minutę. Bez kalibracji błędy pomiarów kąta sięgały 30 stopni.

Całkowity koszt wykonania takiego czujnika jest około 3 razy mniejszy niż jakichkolwiek dostępnych w handlu. Zazwyczaj można kupić osobno czujniki przyspieszenia lub pola magnetycznego za cenę przewyższającą układ LSM303DLH.

Jeżeli masz jakiekolwiek pytania - napisz do mnie. Jestem pewien, że w opisie pominąłem jakieś ważne informacje.

Wideo


Galeria

Kod programu

//author: Krzysztof Kapusta, All rights reserved /*****************************************************************/ /* LSM303DLH */ /* Digital Accelerometer & Magnetometer */ /* for */ /* Lego Mindstorms NXT 2.0 */ /* */ /* Author: Krzysztof Kapusta */ /* Release date: 12.02.2012 */ /* Version: 1.0 */ /* */ /* I cannot guarantee that this library will work */ /* and I cannot guarantee that it will not cause damage. */ /* This file is still under development. */ /* */ /* If you have any problems using this file or */ /* you've found a bug, please report to me to */ /* admin@legorobot.pl */ /*****************************************************************/ #ifndef _ACCMAG_NXC_ #define _ACCMAG_NXC_ // Next definition is for adding delay before every I2C command. If you don't want that, just comment next line #define _ACCMAG_PROBLEMS #define _ACCMAG_DELAY 2 struct Vector { s16 X,Y,Z; }; ///////////////////////////// MAGNETOMETER INTERFACE /////////////////////////// #define MAG_ADR 0x3C #define CRA_REG_M 0x00 #define CRB_REG_M 0x01 #define MR_REG_M 0x02 #define OUT_X_H_M 0x03 #define OUT_X_L_M 0x04 #define OUT_Y_H_M 0x05 #define OUT_Y_L_M 0x06 #define OUT_Z_H_M 0x07 #define OUT_Z_L_M 0x08 #define SR_REG_M 0x09 #define IRA_REG_M 0x0A #define IRB_REG_M 0x0B #define IRC_REG_M 0x0C #define IRA_REG_M_RESPONSE 0x48 #define IRB_REG_M_RESPONSE 0x34 #define IRC_REG_M_RESPONSE 0x33 #define CRA_REG_M_DATA_OUTPUT_RATE_MASK 0x1C #define CRA_REG_M_CONF_MASK 0x03 #define CRB_REG_M_GAIN_MASK 0xE0 #define MR_REG_M_MODE_MASK 0x03 #define SR_REG_M_REGULATOR_ENABLED_MASK 0x04 #define SR_REG_M_OUTPUT_REGISTER_LOCK_MASK 0x02 #define SR_REG_M_READY_MASK 0x01 #define MAG_DATA_OUTPUT_RATE_0_75_HZ 0x00 #define MAG_DATA_OUTPUT_RATE_1_5_HZ 0x04 #define MAG_DATA_OUTPUT_RATE_3_HZ 0x08 #define MAG_DATA_OUTPUT_RATE_7_5_HZ 0x0C #define MAG_DATA_OUTPUT_RATE_15_HZ 0x10 #define MAG_DATA_OUTPUT_RATE_30_HZ 0x14 #define MAG_DATA_OUTPUT_RATE_75_HZ 0x18 #define MAG_CONF_NORMAL 0x00 #define MAG_CONF_POSITIVE_BIAS 0x01 #define MAG_CONF_NEGATIVE_BIAS 0x02 #define MAG_GAIN_1_3_GAUSS 0x20 #define MAG_GAIN_1_9_GAUSS 0x40 #define MAG_GAIN_2_5_GAUSS 0x60 #define MAG_GAIN_4_0_GAUSS 0x80 #define MAG_GAIN_4_7_GAUSS 0xA0 #define MAG_GAIN_5_6_GAUSS 0xC0 #define MAG_GAIN_8_1_GAUSS 0xE0 #define MAG_MODE_CONTINUOUS 0x00 #define MAG_MODE_SINGLE 0x01 #define MAG_MODE_SLEEP 0x03 /* Output data rate for the magnetometer */ enum MagDataOutputRate { MRATE_0_75_HZ = MAG_DATA_OUTPUT_RATE_0_75_HZ, // 0.75 Hz MRATE_1_5_HZ = MAG_DATA_OUTPUT_RATE_1_5_HZ, // 1.5 Hz MRATE_3_HZ = MAG_DATA_OUTPUT_RATE_3_HZ, // 3 Hz MRATE_7_5_HZ = MAG_DATA_OUTPUT_RATE_7_5_HZ, // 7.5 Hz MRATE_15_HZ = MAG_DATA_OUTPUT_RATE_15_HZ, // 15 Hz MRATE_30_HZ = MAG_DATA_OUTPUT_RATE_30_HZ, // 30 Hz MRATE_75_HZ = MAG_DATA_OUTPUT_RATE_75_HZ }; // 75 Hz /* Configuration of the magnetometer */ enum MagConfiguration { MNORMAL = MAG_CONF_NORMAL, // normal confuguration for normal use MPOSITIVE_BIAS = MAG_CONF_POSITIVE_BIAS, // for testing MNEGATIVE_BIAS = MAG_CONF_NEGATIVE_BIAS }; // for testing /* Scale of the magnetometer - it's the maximum value that it will read correctly The less the more accurate readings */ enum MagGain { MGAIN_1_3_GAUSS = MAG_GAIN_1_3_GAUSS, // 1.3 Gauss MGAIN_1_9_GAUSS = MAG_GAIN_1_9_GAUSS, // 1.9 Gauss MGAIN_2_5_GAUSS = MAG_GAIN_2_5_GAUSS, // 2.5 Gauss MGAIN_4_0_GAUSS = MAG_GAIN_4_0_GAUSS, // 4.0 Gauss MGAIN_4_7_GAUSS = MAG_GAIN_4_7_GAUSS, // 4.7 Gauss MGAIN_5_6_GAUSS = MAG_GAIN_5_6_GAUSS, // 5.6 Gauss MGAIN_8_1_GAUSS = MAG_GAIN_8_1_GAUSS }; // 8.1 Gauss /* Magnetometer's mode */ enum MagMode { MCONTINUOUS = MAG_MODE_CONTINUOUS, // Continuous reading MSINGLE = MAG_MODE_SINGLE, // Just one reading MSLEEP = MAG_MODE_SLEEP }; // Sleep mode - no readings /* Initialization of the magnetometer. This function must be called before any reading from the sensor. Return: false if something went wrong; true otherwise */ inline bool SetSensorMagnetic(const byte & port, const MagMode & mode = MCONTINUOUS, const MagDataOutputRate & outputRate = MRATE_75_HZ, const MagGain & gain = MGAIN_1_3_GAUSS, const MagConfiguration & configuration = MNORMAL) { SetSensorType(port, IN_TYPE_LOWSPEED); SetSensorMode(port, IN_MODE_RAW); ResetSensor(port); byte out; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, IRA_REG_M, out); if (out != IRA_REG_M_RESPONSE || I2CCheckStatus(port)) return false; byte val = outputRate | configuration; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (WriteI2CRegister(port, MAG_ADR, CRA_REG_M, val)) return false; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (I2CCheckStatus(port)) return false; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (WriteI2CRegister(port, MAG_ADR, CRB_REG_M, gain)) return false; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (I2CCheckStatus(port)) return false; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (WriteI2CRegister(port, MAG_ADR, MR_REG_M, mode)) return false; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (I2CCheckStatus(port)) return false; return true; } /* Returns output rate for the magnetometer */ inline MagDataOutputRate MagGetOutputRate(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, CRA_REG_M, read); return (read & CRA_REG_M_DATA_OUTPUT_RATE_MASK); } /* Returns magnetometer's configuration */ inline MagConfiguration MagGetConfiguration(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, CRA_REG_M, read); return (read & CRA_REG_M_CONF_MASK); } /* Returns current magnetometer's gain */ inline MagGain MagGetGain(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, CRB_REG_M, read); return (read & CRB_REG_M_GAIN_MASK); } /* Returns magnetometer operating mode */ inline MagMode MagGetMode(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, MR_REG_M, read); return (read & MR_REG_M_MODE_MASK); } /* Checks if the internal regulator is enabled */ inline bool MagIsRegulatorEnabled(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, SR_REG_M, read); return (read & SR_REG_M_REGULATOR_ENABLED_MASK); } /* Checks if output register is locked - it means that there is a writing pending in the device */ inline bool MagIsOutputRegisterLocked(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, SR_REG_M, read); return (read & SR_REG_M_OUTPUT_REGISTER_LOCK_MASK); } /* Ckecks if there's new reading available */ inline bool MagIsReady(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, SR_REG_M, read); return (read & SR_REG_M_READY_MASK); } /* Sets new output rate for the magnetometer. Return: error code (see CommLSCheckStatusType for more information) */ inline char MagSetOutputRate(const byte & port, const MagDataOutputRate & outputRate) { byte write = (outputRate | MagGetConfiguration(port)); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, MAG_ADR, CRA_REG_M, write); } /* Sets new configuration for the magnetometer. Return: error code (see CommLSCheckStatusType for more information) */ inline byte MagSetConfiguration(const byte & port, const MagConfiguration & configuration) { byte write = (configuration | MagGetOutputRate(port)); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, MAG_ADR, CRA_REG_M, write); } /* Sets new gain for the magnetometer. Return: error code (see CommLSCheckStatusType for more information) */ inline byte MagSetGain(const byte & port, const MagGain & gain) { #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, MAG_ADR, CRB_REG_M, gain); } /* Sets new mode for the magnetometer. Return: error code (see CommLSCheckStatusType for more information) */ inline byte MagSetMode(const byte & port, const MagMode & mode) { #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, MAG_ADR, MR_REG_M, mode); } /* Scaled raw value from the magnetometer. The scale is 1 Gauss. You need to provide scale in which the measurment was taken. */ inline VectorType MagRawToScaled(const Vector & raw, const MagGain & gain) { VectorType ret; switch (gain) { case MGAIN_1_3_GAUSS: ret.X = 1.0*raw.X/1055.0; ret.Y = 1.0*raw.Y/1055.0; ret.Z = 1.0*raw.Z/950.0; break; case MGAIN_1_9_GAUSS: ret.X = 1.0*raw.X/795.0; ret.Y = 1.0*raw.Y/795.0; ret.Z = 1.0*raw.Z/710.0; break; case MGAIN_2_5_GAUSS: ret.X = 1.0*raw.X/635.0; ret.Y = 1.0*raw.Y/635.0; ret.Z = 1.0*raw.Z/570.0; break; case MGAIN_4_0_GAUSS: ret.X = 1.0*raw.X/430.0; ret.Y = 1.0*raw.Y/430.0; ret.Z = 1.0*raw.Z/385.0; break; case MGAIN_4_7_GAUSS: ret.X = 1.0*raw.X/375.0; ret.Y = 1.0*raw.Y/375.0; ret.Z = 1.0*raw.Z/335.0; break; case MGAIN_5_6_GAUSS: ret.X = 1.0*raw.X/320.0; ret.Y = 1.0*raw.Y/320.0; ret.Z = 1.0*raw.Z/295.0; break; case MGAIN_8_1_GAUSS: ret.X = 1.0*raw.X/230.0; ret.Y = 1.0*raw.Y/230.0; ret.Z = 1.0*raw.Z/205.0; break; } return ret; } /* Reads raw value from the magnetometer */ inline Vector ReadSensorMagRaw(const byte & port) { Vector result; u16 num; byte out; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_X_H_M, out); num = out<<8; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_X_L_M, out); num |= out; result.X = (num & 0x8000 ? num - 0x10000 : num); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_Y_H_M, out); num = out<<8; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_Y_L_M, out); num |= out; result.Y = (num & 0x8000 ? num - 0x10000 : num); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_Z_H_M, out); num = out<<8; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, MAG_ADR, OUT_Z_L_M, out); num |= out; result.Z = (num & 0x8000 ? num - 0x10000 : num); return result; } /* Reads scaled value from the magnetometer. You should specify current gain for the device. It reduces communication to the device Return unit: 1 Gauss */ VectorType ReadSensorMagScaledEx(const byte & port, const MagGain & gain) { VectorType v = MagRawToScaled(ReadSensorMagRaw(port), gain); return v; } /* Reads scaled value from the magnetometer */ VectorType ReadSensorMagScaled(const byte & port) { VectorType v = MagRawToScaled(ReadSensorMagRaw(port), MagGetGain(port)); return v; } /////////////////////////// ACCELEROMETER INTERFACE /////////////////////////// #define ACC_ADR 0x30 #define CTRL_REG1_A 0x20 #define CTRL_REG2_A 0x21 #define CTRL_REG4_A 0x23 #define HP_FILTER_RESET_A 0x25 #define REFERENCE_A 0x26 #define STATUS_REG_A 0x27 #define OUT_X_L_A 0x28 #define OUT_X_H_A 0x29 #define OUT_Y_L_A 0x2A #define OUT_Y_H_A 0x2B #define OUT_Z_L_A 0x2C #define OUT_Z_H_A 0x2D #define CTRL_REG1_A_POWER_MODE_MASK 0xE0 #define CTRL_REG1_A_DATA_RATE_MASK 0x18 #define CTRL_REG1_A_Z_ENABLE_MASK 0x04 #define CTRL_REG1_A_Y_ENABLE_MASK 0x02 #define CTRL_REG1_A_X_ENABLE_MASK 0x01 #define CTRL_REG2_A_BOOT_MASK 0x80 #define CTRL_REG2_A_HIGHPASS_MODE_MASK 0x60 #define CTRL_REG2_A_FILTERED_DATA_MASK 0x10 #define CTRL_REG2_A_HIGHPASS_FREQ_MASK 0x03 #define CTRL_REG4_A_BLOCK_DATA_UPDATE_MASK 0x80 #define CTRL_REG4_A_BIG_LITTLE_ENDIAN_MASK 0x40 #define CTRL_REG4_A_SCALE_MASK 0x30 #define CTRL_REG4_A_SELF_TEST_SIGN_MASK 0x08 #define CTRL_REG4_A_SELF_TEST_ENABLE_MASK 0x20 #define STATUS_REG_A_XYZ_OVERRUN_MASK 0x80 #define STATUS_REG_A_Z_OVERRUN_MASK 0x40 #define STATUS_REG_A_Y_OVERRUN_MASK 0x20 #define STATUS_REG_A_X_OVERRUN_MASK 0x10 #define STATUS_REG_A_XYZ_NEW_DATA_MASK 0x08 #define STATUS_REG_A_Z_NEW_DATA_MASK 0x04 #define STATUS_REG_A_Y_NEW_DATA_MASK 0x02 #define STATUS_REG_A_X_NEW_DATA_MASK 0x01 #define ACC_MODE_SLEEP 0x00 #define ACC_MODE_NORMAL 0x20 #define ACC_MODE_LOWPOWER_05_HZ 0x40 #define ACC_MODE_LOWPOWER_1_HZ 0x60 #define ACC_MODE_LOWPOWER_2_HZ 0x80 #define ACC_MODE_LOWPOWER_5_HZ 0xA0 #define ACC_MODE_LOWPOWER_10_HZ 0xC0 #define ACC_DATA_OUTPUT_RATE_50_HZ 0x00 #define ACC_DATA_OUTPUT_RATE_100_HZ 0x08 #define ACC_DATA_OUTPUT_RATE_400_HZ 0x10 #define ACC_DATA_OUTPUT_RATE_1000_HZ 0x18 #define ACC_HIGHPASS_CONFIGURATION_NORMAL 0x00 #define ACC_HIGHPASS_CONFIGURATION_REFERENCE 0x20 #define ACC_SCALE_2G 0x00 #define ACC_SCALE_4G 0x10 #define ACC_SCALE_8G 0x30 /* Accelerometer mode */ enum AccMode { ASLEEP = ACC_MODE_SLEEP, // sleep mode ANORMAL = ACC_MODE_NORMAL, // normal full power mode ALOWPOWER_05_HZ = ACC_MODE_LOWPOWER_05_HZ, // low power mode, data rate 0.5 Hz ALOWPOWER_1_HZ = ACC_MODE_LOWPOWER_1_HZ, // low power mode, data rate 1 Hz ALOWPOWER_2_HZ = ACC_MODE_LOWPOWER_2_HZ, // low power mode, data rate 2 Hz ALOWPOWER_5_HZ = ACC_MODE_LOWPOWER_5_HZ, // low power mode, data rate 5 Hz ALOWPOWER_10_HZ = ACC_MODE_LOWPOWER_10_HZ }; // low power mode, data rate 10 Hz /* Data output rate for the accelerometer This setting is valid only in mode ANORMAL */ enum AccDataOutputRate { ARATE_50_HZ = ACC_DATA_OUTPUT_RATE_50_HZ, // 50 Hz ARATE_100_HZ = ACC_DATA_OUTPUT_RATE_100_HZ, // 100 Hz ARATE_400_HZ = ACC_DATA_OUTPUT_RATE_400_HZ, // 400 Hz ARATE_1000_HZ = ACC_DATA_OUTPUT_RATE_1000_HZ }; // 1000 Hz /* Setting for the built-in high-pass filter */ enum AccHighpassConfiguration { AHIGHPASS_NORMAL = ACC_HIGHPASS_CONFIGURATION_NORMAL, AHIGHPASS_REFERENCE = ACC_HIGHPASS_CONFIGURATION_REFERENCE }; /* Accelerometer's scale - max reading you can get */ enum AccScale { ASCALE_2G = ACC_SCALE_2G, // 2 g ASCALE_4G = ACC_SCALE_4G, // 4 g ASCALE_8G = ACC_SCALE_8G }; // 8 g /* Initializing acceleration sensor. Must be done before reading from the sensor. */ inline bool SetSensorAcceleration(const byte & port, const AccMode & mode = ANORMAL, const AccScale & scale = ASCALE_2G, const AccDataOutputRate & outputRate = ARATE_400_HZ) { SetSensorType(port, IN_TYPE_LOWSPEED); SetSensorMode(port, IN_MODE_RAW); ResetSensor(port); byte wr = mode | outputRate | CTRL_REG1_A_Z_ENABLE_MASK | CTRL_REG1_A_Y_ENABLE_MASK | CTRL_REG1_A_X_ENABLE_MASK; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (WriteI2CRegister(port, ACC_ADR, CTRL_REG1_A, wr)) return false; if (I2CCheckStatus(port)) return false; wr = CTRL_REG4_A_BLOCK_DATA_UPDATE_MASK | scale; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif if (WriteI2CRegister(port, ACC_ADR, CTRL_REG4_A, wr)) return false; if (I2CCheckStatus(port)) return false; return true; } /* Returns current device's scale */ inline AccScale AccGetScale(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, CTRL_REG4_A, read); return (read & CTRL_REG4_A_SCALE_MASK); } /* Sets new scale for the accelerometer. Return: error code (see CommLSCheckStatusType for more information) */ inline char AccSetScale(const byte & port, const AccScale & scale) { byte prev; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif char out = ReadI2CRegister(port, ACC_ADR, CTRL_REG4_A, prev); if (out != 0) return out; prev &= (~CTRL_REG4_A_SCALE_MASK); prev |= scale; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, ACC_ADR, CTRL_REG4_A, prev); } /* Returns current device's mode */ inline AccDataOutputRate AccGetOutputRate(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, CTRL_REG1_A, read); return (read & CTRL_REG1_A_DATA_RATE_MASK); } /* Sets new data output rate for the accelerometer. Return: error code (see CommLSCheckStatusType for more information) */ inline char AccSetOutputRate(const byte & port, const AccDataOutputRate & rate) { byte prev; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif char out = ReadI2CRegister(port, ACC_ADR, CTRL_REG1_A, prev); if (out != 0) return out; prev &= (~CTRL_REG1_A_DATA_RATE_MASK); prev |= rate; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, ACC_ADR, CTRL_REG1_A, prev); } /* Returns current device's mode */ inline AccMode AccGetMode(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, CTRL_REG1_A, read); return (read & CTRL_REG1_A_POWER_MODE_MASK); } /* Sets new power mode for the accelerometer. Return: error code (see CommLSCheckStatusType for more information) */ inline char AccSetMode(const byte & port, const AccMode & mode) { byte prev; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif char out = ReadI2CRegister(port, ACC_ADR, CTRL_REG1_A, prev); if (out != 0) return out; prev &= (~CTRL_REG1_A_POWER_MODE_MASK); prev |= mode; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif return WriteI2CRegister(port, ACC_ADR, CTRL_REG1_A, prev); } /* Checks if there is a new set of readings available for reading */ inline bool AccIsReady(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, STATUS_REG_A, read); return (read & STATUS_REG_A_XYZ_NEW_DATA_MASK); } /* Checks if previous reading on the device was overran by a new one before it was read */ inline bool AccOverrun(const byte & port) { byte read; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, STATUS_REG_A, read); return (read & STATUS_REG_A_XYZ_OVERRUN_MASK); } inline VectorType AccRawToScaled(const Vector & raw, const AccScale & scale) { VectorType ret; switch (scale) { case ACC_SCALE_2G: ret.X = 1.0*raw.X/1000.0; ret.Y = 1.0*raw.Y/1000.0; ret.Z = 1.0*raw.Z/1000.0; break; case ACC_SCALE_4G: ret.X = 1.0*raw.X/500.0; ret.Y = 1.0*raw.Y/500.0; ret.Z = 1.0*raw.Z/500.0; break; case ACC_SCALE_8G: ret.X = 1.0*raw.X/256.41; ret.Y = 1.0*raw.Y/256.41; ret.Z = 1.0*raw.Z/256.41; break; } return ret; } /* Reads raw values from the sensor */ inline Vector ReadSensorAccRaw(const byte & port) { Vector result; u16 num; byte out; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_X_H_A, out); num = out<<4; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_X_L_A, out); num |= (out>>4); result.X = (num & 0x800 ? num - 0x1000 : num); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_Y_H_A, out); num = out<<4; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_Y_L_A, out); num |= (out>>4); result.Y = (num & 0x800 ? num - 0x1000 : num); #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_Z_H_A, out); num = out<<4; #ifdef _ACCMAG_PROBLEMS Wait(_ACCMAG_DELAY); #endif ReadI2CRegister(port, ACC_ADR, OUT_Z_L_A, out); num |= out>>4; result.Z = (num & 0x800 ? num - 0x1000 : num); return result; } /* Reads scaled values from the sensor provided that you know the scale in which the device is operating. Look at AccScale. Using this function reduces the communication to the sensor. */ VectorType ReadSensorAccScaledEx(const byte & port, const AccScale & scale) { VectorType v = AccRawToScaled(ReadSensorAccRaw(port), scale); return v; } /* Reads scaled values from the sensor. Unit: 1g */ VectorType ReadSensorAccScaled(const byte & port) { VectorType v = AccRawToScaled(ReadSensorAccRaw(port), AccGetScale(port)); return v; } ////////////////////////// MAG & ACC INTERFACE //////////////////////////////// inline bool SetSensorAccMag(const byte & port) { SetSensorMagnetic(port); SetSensorAcceleration(port); } inline int GetHeading(const VectorType & accReading, const VectorType & magReading, const VectorType & front) { VectorType east, north; VectorCross(magReading, accReading, east); VectorCross(accReading, east, north); return atan2(VectorDot(east, front), VectorDot(north, front)) * 180.0 / PI; } inline void ReadSensorAccMagSteady(const byte & port, const long & timeMs, VectorType & accReading, VectorType & magReading) { long count = 0, starttime = CurrentTick(), mX=0, mY=0, mZ=0, aX=0, aY=0, aZ=0; Vector M, A, reading; while (CurrentTick() - starttime < timeMs) { while (!MagIsReady(port)); reading = ReadSensorMagRaw(port); mX += reading.X; mY += reading.Y; mZ += reading.Z; reading = ReadSensorAccRaw(port); aX += reading.X; aY += reading.Y; aZ += reading.Z; count++; } A.X = aX/count; A.Y = aY/count; A.Z = aZ/count; M.X = mX/count; M.Y = mY/count; M.Z = mZ/count; accReading = AccRawToScaled(A, AccGetScale(port)); magReading = MagRawToScaled(M, MagGetGain(port)); } #endif

Pokaż/ukryj cały kod

DOWNLOAD

Komentarze

Hi, Sir. Where can I learn the tilt compensation and every stuff about control the compass and accelerometer? thank you.

Dodane przez: mike | 2016-04-19

Koszt całości to około 140 zł. Można zrobić sam akcelerometr. Wystarczy jakikolwiek moduł obsługujący magistralę I2C (lub TWI)

Dodane przez: Admin | 2012-04-13

Ile dokładnie kosztuje? Czy można by zrobić sam akcelerometr?

Dodane przez: Lego mindstorms | 2012-04-13

Dodaj swój komentarz