Skip to content

Instantly share code, notes, and snippets.

@ende76
Created January 18, 2013 13:11
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save ende76/4564465 to your computer and use it in GitHub Desktop.
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.
// 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);
}
@tuga89pt
Copy link

I am doing this same project but with a stepper motor by chance you can show me your schematic?

my email is: ricardojcportugal@hotmail.com

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment