I've been using a spirit level for a while to measure tilt, but wanted to get more accurate. So I found a way to use a gyroscope/accelerometer breakout to measure it.
This involves a separate arduino outside of the duet. The code is from the example here: https://github.com/TKJElectronics/KalmanFilter
I used the adafruit breakout here: https://learn.adafruit.com/adafruit-lsm9ds0-accelerometer-gyro-magnetometer-9-dof-breakouts/wiring-and-test?view=all#overview
You just install the code on the arduino (customize for your breakout if you use a different one) and clip the breakout rigidly to your effector. Then move your effector around, and measure by watching the serial monitor. Definitive numbers, a measurement of tilt at any point on your printer. (The code at its start measures the current angle, and uses that as the reference, it gives two sets of two numbers… angle measures, and deviation from the reference).
My printer measures a pitch range of -0.1 to 0.1 degrees, and a roll range of -0.1 to 0.4 degrees of tilt. (My "worst" point is when my effector is at 0,-135)
Here's the code:
[[language]]
#include <wire.h>#include <spi.h>#include <adafruit_lsm9ds0.h>#include <adafruit_sensor.h>#include <kalman.h>//Arduino Mega:
//#define LSM9DS0_XM_CS 46
//#define LSM9DS0_GYRO_CS 44
//Adafruit_LSM9DS0 lsm = Adafruit_LSM9DS0(LSM9DS0_XM_CS, LSM9DS0_GYRO_CS);
Adafruit_LSM9DS0 lsm = Adafruit_LSM9DS0();
#define RESTRICT_PITCH
Kalman kalmanX;
Kalman kalmanY;
// Create sensor events (these get "filled" with data)
sensors_event_t accel, mag, gyro, temp;
float accX;
float accY;
float accZ;
float gyroX = 0;
float gyroY = 0;
float gyroZ = 0;
int16_t tempRaw;
float sRoll;
float sPitch;
int maxDev=10;
float dev;
float gyroXangle, gyroYangle;
float compAngleX, compAngleY;
float kalAngleX, kalAngleY;
uint32_t timer;
char AngType = 'R';
float CompVal = 0.02;
// Setup for Sensors
void configureSensor()
{
lsm.setupAccel(lsm.LSM9DS0_ACCELRANGE_2G);
lsm.setupMag(lsm.LSM9DS0_MAGGAIN_2GAUSS);
lsm.setupGyro(lsm.LSM9DS0_GYROSCALE_245DPS);
}
void setup()
{
// Set SS to high and OUTPUT so a connected chip will be "deselected" by default
//digitalWrite(53, HIGH);
//pinMode(53, OUTPUT);
Serial.begin(9600);
Serial.println("Getting started");
// Try to initialise and warn if we couldn't detect the chip
if (!lsm.begin())
{
Serial.println("Oops ... unable to initialize the LSM9DS0\. Check your wiring!");
while (1);
}
Serial.println("Found LSM9DS0 9DOF");
Serial.println("");
configureSensor;
delay(100);
lsm.getEvent(&accel, &mag, &gyro, &temp);
accX = accel.acceleration.x;
accY = accel.acceleration.y;
accZ = accel.acceleration.z;
gyroX = gyro.gyro.x;
gyroY = gyro.gyro.y;
gyroZ - gyro.gyro.z;
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
kalmanX.setAngle(roll); // Set starting angle
kalmanY.setAngle(pitch);
gyroXangle = roll;
gyroYangle = pitch;
compAngleX = roll;
compAngleY = pitch;
sRoll = roll;
sPitch = pitch;
timer = micros();
}
void loop(){
lsm.getEvent(&accel, &mag, &gyro, &temp);
accX = accel.acceleration.x;
accY = accel.acceleration.y;
accZ = accel.acceleration.z;
gyroX = gyro.gyro.x;
gyroY = gyro.gyro.y;
gyroZ - gyro.gyro.z;
double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
timer = micros();
#ifdef RESTRICT_PITCH // Eq. 25 and 26
double roll = atan2(accY, accZ) * RAD_TO_DEG;
double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
#else // Eq. 28 and 29
double roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
#endif
double gyroXrate = gyroX / 131.0; // Convert to deg/s
double gyroYrate = gyroY / 131.0; // Convert to deg/s
#ifdef RESTRICT_PITCH
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
kalmanX.setAngle(roll);
compAngleX = roll;
kalAngleX = roll;
gyroXangle = roll;
} else
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleX) > 90)
gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
#else
// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
kalmanY.setAngle(pitch);
compAngleY = pitch;
kalAngleY = pitch;
gyroYangle = pitch;
} else
kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
if (abs(kalAngleY) > 90)
gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
#endif
gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
gyroYangle += gyroYrate * dt;
//gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
//gyroYangle += kalmanY.getRate() * dt;
compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
// Reset the gyro angle when it has drifted too much
if (gyroXangle < -180 || gyroXangle > 180)
gyroXangle = kalAngleX;
if (gyroYangle < -180 || gyroYangle > 180)
gyroYangle = kalAngleY;
/* Print Data */
#if 0 // Set to 1 to activate
Serial.print(accX); Serial.print("\t");
Serial.print(accY); Serial.print("\t");
Serial.print(accZ); Serial.print("\t");
Serial.print(gyroX); Serial.print("\t");
Serial.print(gyroY); Serial.print("\t");
Serial.print(gyroZ); Serial.print("\t");
Serial.print("\t");
#endif
// Serial.print(roll); Serial.print("\t");
// Serial.print(gyroXangle); Serial.print("\t");
// Serial.print(compAngleX); Serial.print("\t");
Serial.print(kalAngleX); Serial.print("\t");
//if((kalAngleX > (sRoll+maxDev))||(kalAngleX < (sRoll-maxDev))){
// this is your emergency stop
//}else{
dev = sRoll-kalAngleX;
Serial.print(dev);
//}
Serial.print("\t\|\t");
// Serial.print(pitch); Serial.print("\t");
// Serial.print(gyroYangle); Serial.print("\t");
// Serial.print(compAngleY); Serial.print("\t");
Serial.print(kalAngleY); Serial.print("\t");
//if((kalAngleY > (sPitch+maxDev))||(kalAngleY < (sPitch-maxDev))){
// this is your emergency stop
//}else{
dev = sPitch-kalAngleY;
Serial.print(dev);
//}
//#if 0 // Set to 1 to print the temperature
// Serial.print("\t");
// double temperature = (double)tempRaw / 340.0 + 36.53;
// Serial.print(temperature); Serial.print("\t");
//#endif
Serial.print("\r\n");
delay(2);
}</kalman.h></adafruit_sensor.h></adafruit_lsm9ds0.h></spi.h></wire.h>
