Created
January 18, 2013 13:11
-
-
Save ende76/4564465 to your computer and use it in GitHub Desktop.
Small example on how to read input from a LIS331HH accelerometer chip, and then to use that input to replicate the orientation of the chip using a servo.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
// Arduino UNO system clock speed: 16 MHz | |
// max SPI clock frequency: 10 MHz | |
// shift order: MSB first | |
// SPC idle level: HIGH | |
// data samples capture edge: rising | |
#include <SPI.h> | |
#include <Servo.h> | |
#define PIN_SLAVE_SELECT 10 | |
#define PIN_SERVO_SIGNAL 2 | |
#define SERVO_ANGLE_MAX 180 | |
#define SERVO_ANGLE_MIN 0 | |
// represent servo as an object | |
Servo myServo; | |
// maintain servo angle state, initialize in the middle of the range | |
int angle = (SERVO_ANGLE_MAX + SERVO_ANGLE_MIN) / 2; | |
#define ANGLE_TOLERANCE 2.0 | |
// masks for read/write accesses | |
const byte MASK_READ = 0b10000000; | |
const byte MASK_WRITE = 0b00000000; | |
// register addresses | |
// control register 1 [rw] | |
// bits: PM2 | PM1 | PM0 | DR1 | DR0 | Zen | Yen | Xen | |
// PM2 - PM0: | |
// Power mode selection. Default value: 000 | |
// | |
// BITS Mode Select Output data rate [Hz] | |
// 000 Power-down -- | |
// 001 Normal mode ODR | |
// 010 Low-power 0.5 | |
// 011 Low-power 1 | |
// 100 Low-power 2 | |
// 101 Low-power 5 | |
// 110 Low-power 10 | |
// DR1 - DR0: | |
// In normal mode, select the data rate at which acceleration samples are produced. | |
// In low-power mode, define output data resolution. | |
// | |
// BITS ODR Low-pass filter cut-off frequency [Hz] | |
// 00 50 37 | |
// 01 100 74 | |
// 10 400 292 | |
// 11 1000 780 | |
// Zen/Yen/Xen: | |
// X/Y/Z axis enable. Default value: 1 (0: axis enabled, 1: axis disabled) | |
const byte REG_CTRL_REG1 = 0b00100000; | |
const byte REG_CTRL_REG2 = 0b00100001; | |
const byte REG_CTRL_REG4 = 0b00100011; | |
// X-axis acceleration data. The value is expressed as two's complement. | |
const byte REG_OUT_X_L = 0b00101000; | |
const byte REG_OUT_X_H = 0b00101001; | |
// Y-axis acceleration data. The value is expressed as two's complement. | |
const byte REG_OUT_Y_L = 0b00101010; | |
const byte REG_OUT_Y_H = 0b00101011; | |
// Z-axis acceleration data. The value is expressed as two's complement. | |
const byte REG_OUT_Z_L = 0b00101100; | |
const byte REG_OUT_Z_H = 0b00101101; | |
void setup() { | |
Serial.begin(115200); | |
// set up servo | |
myServo.attach(PIN_SERVO_SIGNAL); | |
myServo.write(angle); | |
// set up SPI communication | |
pinMode(PIN_SLAVE_SELECT, OUTPUT); | |
digitalWrite(PIN_SLAVE_SELECT, HIGH); | |
SPI.begin(); | |
SPI.setBitOrder(MSBFIRST); | |
SPI.setDataMode(SPI_MODE3); | |
SPI.setClockDivider(SPI_CLOCK_DIV16); | |
reboot(); | |
delay(100); | |
// set up accelerometer | |
setNormalMode(); | |
setHighPassFilterOff(); | |
setFullScale(); | |
delay(50); | |
} | |
void loop() { | |
double x = readX(); | |
double z = readZ(); | |
double phi = constrain(90.0 + acos(-z / sqrt(x*x+z*z)) * ((x > 0) ? 360.0 : -360.0) / TWO_PI, 0.0, 180.0); | |
if (abs(angle - phi) > ANGLE_TOLERANCE) { | |
myServo.write(phi); | |
angle = phi; | |
} | |
delay(10); | |
} | |
double readAccelerationRegister(byte address) { | |
return (double(readRegister(address) | (readRegister(address + 1) << 8)) | |
+ ((address == REG_OUT_X_L) ? 32.0000 : 0.0) | |
+ ((address == REG_OUT_Z_L) ? -149.3125 : 0.0) | |
) * 48.0 / 65535.0; | |
} | |
byte readRegister(byte address) { | |
byte result; | |
// call LIS331HH to attention | |
digitalWrite(PIN_SLAVE_SELECT, LOW); | |
SPI.transfer(MASK_READ | address); | |
result = SPI.transfer(0); | |
// release LIS331HH | |
digitalWrite(PIN_SLAVE_SELECT, HIGH); | |
return result; | |
} | |
byte readCtrl1() { | |
return readRegister(REG_CTRL_REG1); | |
} | |
byte readCtrl2() { | |
return readRegister(REG_CTRL_REG2); | |
} | |
double readX() { | |
return readAccelerationRegister(REG_OUT_X_L); | |
} | |
double readY() { | |
return readAccelerationRegister(REG_OUT_Y_L); | |
} | |
double readZ() { | |
return readAccelerationRegister(REG_OUT_Z_L); | |
} | |
void reboot() { | |
writeRegister(REG_CTRL_REG2, 0b10000000); | |
} | |
void setFullScale() { | |
writeRegister(REG_CTRL_REG4, 0b10110000); | |
} | |
void setHighPassFilterOff() { | |
writeRegister(REG_CTRL_REG2, 0b00000000); | |
} | |
void setNormalMode() { | |
writeRegister(REG_CTRL_REG1, 0b00100111); | |
} | |
void writeRegister(byte address, byte flags) { | |
// call LIS331HH to attention | |
digitalWrite(PIN_SLAVE_SELECT, LOW); | |
SPI.transfer(MASK_WRITE | address); | |
SPI.transfer(flags); | |
// release LIS331HH | |
digitalWrite(PIN_SLAVE_SELECT, HIGH); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
I am doing this same project but with a stepper motor by chance you can show me your schematic?
my email is: [email protected]