hello,
I am trying to use the x,y,z from an accelerometer in a function type code to hopefully give me some roll and pitch angles that I can use to correct a hexapod robot. although I cannot get them to work. I am new to programming so its probably fairly simple:
there's this code from the adafruit LIS3DH:
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_LIS3DH.h>
#include <Adafruit_Sensor.h>
// Used for software SPI
#define LIS3DH_CLK 52
#define LIS3DH_MISO 50
#define LIS3DH_MOSI 51
// Used for hardware & software SPI
#define LIS3DH_CS 53
// software SPI
Adafruit_LIS3DH lis = Adafruit_LIS3DH(LIS3DH_CS, LIS3DH_MOSI, LIS3DH_MISO, LIS3DH_CLK);
// hardware SPI
//Adafruit_LIS3DH lis = Adafruit_LIS3DH(LIS3DH_CS);
// I2C
//Adafruit_LIS3DH lis = Adafruit_LIS3DH();
#if defined(ARDUINO_ARCH_SAMD)
// for Zero, output on USB Serial console, remove line below if using programming port to program the Zero!
#define Serial SerialUSB
#endif
void setup(void) {
#ifndef ESP8266
while (!Serial); // will pause Zero, Leonardo, etc until serial console opens
#endif
Serial.begin(115200);
Serial.println("LIS3DH test!");
// if (! lis.begin(0x18)) { // change this to 0x19 for alternative i2c address
// Serial.println("Couldnt start");
// while (1);
// }
// Serial.println("LIS3DH found!");
lis.setRange(LIS3DH_RANGE_4_G); // 2, 4, 8 or 16 G!
Serial.print("Range = "); Serial.print(4 << lis.getRange());
Serial.println("G");
}
void loop() {
lis.read(); // get X Y and Z data at once
// Then print out the raw data
Serial.print("X: "); Serial.print(lis.x);
Serial.print(" \tY: "); Serial.print(lis.y);
Serial.print(" \tZ: "); Serial.print(lis.z);
/* Or....get a new sensor event, normalized */
sensors_event_t event;
lis.getEvent(&event);
/* Display the results (acceleration is measured in m/s^2) */
// Serial.print("\t\tX: "); Serial.print(event.acceleration.x);
// Serial.print(" \tY: "); Serial.print(event.acceleration.y);
// Serial.print(" \tZ: "); Serial.print(event.acceleration.z);
// Serial.println(" m/s^2 ");
Serial.println();
delay(200);
and I would like the x,y,z to go into this:
int x, y, z; //three axis acceleration data
double roll = 0.00, pitch = 0.00; //Roll & Pitch are the angles which rotate by the axis X and y
void RP_calculate(){
double x_Buff = float(x);
double y_Buff = float(y);
double z_Buff = float(z);
roll = atan2(y_Buff , z_Buff) * 57.3;
pitch = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3;
}
Also, as an extra how could I define these angles to be something I could add onto a 'servo.write(90+the'X value here')?
any help would be appreciated! thank you
harry