Delta Effector Tilt Measurement

kraegar

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>

kraegar

Some further detail.

My original efforts were to use an accelerometer, but it wasn't giving numbers that looked right. Next I tried a gyro I had, but those drift. Then I discovered the idea of complimentary or kalman filters, which combine the data from the two sources. I picked up the adafruit breakout and started playing, but had mixed results. I'd been using a complimentary filter, but it wasn't reliable, especially with motion in the x/y plane, which registered as high amounts of momentary pitch or roll. The discovery of the kalman library linked about made it trivial. The majority of the code is lifted from the example there. It doesn't do well with extremely fast moves (ie, printing speeds) as the gyro still registers sudden direction changes as momentary pitch or roll.

This is a shame, as one of my original ideas was to use this as a "safety stop" for magball effectors. I'm not entirely giving up on that thought, as it seems ideal (if the effector goes far enough out of plane, trigger a pause/stop) but the rapid motion is a problem for the momentary error, and I haven't sorted out a way to solve that yet.

In the meantime, I hope it provides an effective way for others to measure any tilt in their effector and potentially eliminate it.

kraegar

Some notes about the results.

let it run on a table, see if your gyro has drift over time. The way the code is written I zero it out after a short time (the wait in the setup block). If your gyro has a lot of drift, you may need to increase that time.
my "dev" variable assumes you have the breakout "upright". it will give you weird numbers (-360 degrees) if it's upside down.
Things should work if your breakout is on its side, but I find it works best when it's flat.
I move to the Z height I want to measure at (75mm) and then turn on my arduino and let it settle. Then I move about. The delta motion can definitely throw things off. Perhaps someone else can help fix that with code

Shotfire

Interesting solution, I should but one of my RC autopilots on it and see the results. The autopilot already has the Kalman filters in it along with sensor fusion for the Acc's, Gyros, and Magnetometers which will deal with temperature drift of the Gyro's. Might have to remove the Extruder motor to eliminate magnetic interference. Might also be able to calibrate the Magnetometers with the autopilot in the machine to eliminate the effect of the magnets in the motor. Then read the graph real time and see the results.

kraegar

FYI - disable the magno sensor if you use magball arms. They go kinda nuts.

kraegar

Here's what the setup looks like. Literally just a binder clip. Exact orientation isn't important as long as it doesn't move around during your tests. I periodically move back to 0,0 to make sure drift isn't too bad, and it didn't get bumped. (Initially I measured over 2 degrees of roll, until I realized the sensor had moved)

T3P3Tony

Hi Kraegar

Thats really interesting work, thanks! I am interested in the idea of adding the components to a PCB effector like the one David and I are working on as a fault detection system

kraegar

There are issues. My original thought in this was two fold… one, to measure tilt. I think this does that well. The second was as a "oh, crap" bailout for magball disconnects. Unfortunately at printing speeds the gyro still registers momentary amounts of pitch or roll in excess of 10 degrees, even when constrained to remain perfectly flat. (Before I found the kalman filter, it was over 30 degrees, so it's an improvement, but not enough).

I'm thinking someone smarter than I (or at least better at coding in C) can take it further to resolve that, and it would be an elegant way of serving that purpose.

As it sits, I think it should be an effective diagnostic tool. I just did a test, and backed out a magball to similate an error in effector geometry, and it was very apparent in the numbers, accruing almost 1.75 degrees of tilt.

I'll be interested to see if others take the idea, and where it goes from here, though.

kraegar

Some notes based on questions I've gotten - test your sensor, mine's marked for pitch & roll, but be sure you know which is which before you mount it. The output is Actual Roll, Deviation from starting Roll, Actual Pitch, Deviation from starting pitch… as I have it in the code above. There are lots of other outputs you can enable, just by removing the comments for them. The ones I'm displaying are the kalman angles. For testing, especially with a different sensor, I'd enable more of the output.

maxDev is defined as 10... and not currently used. You can see I've commented out the bits for it in the output section, but that's where you could set it so that if the angle is more than 10 degrees from the start angle, send a hard stop / pause / warning or whatever to your printer.