Акселерометр управляет серво-мотором

Акселерометр ADXL345 отлично подходит для измерения статического ускорения свободного падения, если стоит задача по измерению угла наклона, а также подходит для измерения динамического ускорения, обусловленного движением или ударом. Высокая разрешающая способность (4 mg/LSB) позволяет измерять изменение наклона менее 1 градуса.

Собранное устройство работает следующим образом. Акселерометр ADXL345, работающий в диапазоне +-16g и имеющий разрешение 13-бит, отправляет информацию о положении на микроконтроллерный модуль по шине I2C (может работать и по SPI). Микроконтроллер декодирует данные, полученные от акселерометра, затем выполняет преобразование, используя специализированную функцию atan2.

Затем формируется управляющее воздействие на серво-мотор. Важной особенностью является отсутствие библиотек для серво-мотора.

Тестовый листинг программы приведён ниже.

/*
  Подключение акселерометра по шине I2C:
  VCC: 5V
  GND: ground
  SCL: UNO SCL (А5?)
  SDA: UNO SDA (А4?)
*/

#include <Servo.h>
Servo motor; // Определяем имя серво-привода
#include <Math.h> // Вроде, нужна для atan2
#include <Wire.h>
// Registers for ADXL345
#define ADXL345_ADDRESS (0xA6 >> 1)  // address for device is 8 bit but shift to the
// right by 1 bit to make it 7 bit because the
// wire library only takes in 7 bit addresses
#define ADXL345_REGISTER_XLSB (0x32)

float Angle;      // угол поворота мотора
float prev_Angle; // временная переменная для угла
float value; // промежуточное значение Y

int accelerometer_data[3];
// void because this only tells the chip to send data to its output register
// writes data to the slave's buffer
void i2c_write(int address, byte reg, byte data) {
  // Send output register address
  Wire.beginTransmission(address);
  // Connect to device
  Wire.write(reg);
  // Send data
  Wire.write(data); //low byte
  Wire.endTransmission();
}

// void because using pointers
// microcontroller reads data from the sensor's input register
void i2c_read(int address, byte reg, int count, byte* data) {
  // Used to read the number of data received
  int i = 0;
  // Send input register address
  Wire.beginTransmission(address);
  // Connect to device
  Wire.write(reg);
  Wire.endTransmission();
  // Connect to device
  Wire.beginTransmission(address);
  // Request data from slave
  // Count stands for number of bytes to request
  Wire.requestFrom(address, count);
  while (Wire.available()) // slave may send less than requested
  {
    char c = Wire.read(); // receive a byte as character
    data[i] = c;
    i++;
  }
  Wire.endTransmission();
}

void init_adxl345() {
  byte data = 0;
  i2c_write(ADXL345_ADDRESS, 0x31, 0x0B);   // 13-bit mode  +- 16g
  i2c_write(ADXL345_ADDRESS, 0x2D, 0x08);   // Power register
  i2c_write(ADXL345_ADDRESS, 0x1E, 0x00);   // X
  i2c_write(ADXL345_ADDRESS, 0x1F, 0x00);   // Y
  i2c_write(ADXL345_ADDRESS, 0x20, 0x05);   // Z
  // Проверка работоспособности
  i2c_read(ADXL345_ADDRESS, 0X00, 1, &data);
  if (data == 0xE5)
    Serial.println("It works well!");
  else
    Serial.println("Failure... :(");
}

void read_adxl345() {
  byte bytes[6];
  memset(bytes, 0, 6);
  // Чтение шести байтов из ADXL345
  i2c_read(ADXL345_ADDRESS, ADXL345_REGISTER_XLSB, 6, bytes);
  // Распаковка данных
  for (int i = 0; i < 3; ++i) {
    accelerometer_data[i] = (int)bytes[2 * i] + (((int)bytes[2 * i + 1]) << 8);
  }
}

void setup() {
  motor.attach(9); // Прицепили серво-мотор (9 or 10)
  Wire.begin();
  Serial.begin(9600);
  for (int i = 0; i < 3; ++i)   {
    accelerometer_data[i]  = 0;
  }
  init_adxl345();
}

void react() {
  // Теперь посылаем данные в мотор
  // Angle = map(value, 0, 1.0, 0, 180); // диапазон акселерометра отображается на 0..180 или 0..20 градусов
  Angle = value * 180.0;
  // Angle = constrain(Angle, 1, 180); // ограничиваем значения диапазоном от 1 до 180

  Serial.print("\t");
  Serial.print("Угол: ");
  Serial.print(Angle);
  Serial.println("\t");
  if (Angle != prev_Angle)
  {
    motor.write(Angle); // установить угол поворота мотора
    prev_Angle = Angle; // запомнить текущий угол
  }
}

void loop() {
  read_adxl345();
  Serial.print("ACCEL: ");
  Serial.print(float(accelerometer_data[0]) * 3.9 / 1000); // 3.9mg/LSB scale factor in 13-bit mode
  Serial.print("\t");
  Serial.print(float(accelerometer_data[1]) * 3.9 / 1000);
  Serial.print("\t");
  Serial.print(float(accelerometer_data[2]) * 3.9 / 1000);
  Serial.print("\n");

  // value = float((accelerometer_data[1]) * 3.9 / 1000);

  value = atan2(float((accelerometer_data[1]) * 3.9 / 1000), float((accelerometer_data[0]) * 3.9 / 1000));
  if ( (value > 0) && (value < 1.0)) {
    react ();
   }
  delay(100);
}