Electronic Spirit Level

0 1545 Medium

An electronic spirit level using 'Motion Gestures' to get things horizontal OR to the slope of your choice!

projectImage

Things used in this project

 

Hardware components

HARDWARE LIST
1 DFRobot 6 DOF Sensor - MPU6050
1 Arduino Nano R3
1 Digilent WS2812 Addressable LED Strip

Story

 

This project uses an MPU6050 motion sensor to act as a Spirit Level using some RGB LEDs to indicate offset from level. The real fun comes with the addition of 'Motion Gesture' detection to allow the Spirit Level to be re-levelled about an arbitrary slope. This then becomes the 'Null' level which the Spirit Level measures against. This allows you to check that different surfaces are at the same slope rather than simply horizontal.

This is a SweetMaker StrawberryString project building on the StrawberryString hardware configuration and the SweetMaker framework.

 

Level Detection

 

The SweetMaker MotionSensor library provides the current orientation of the sensor and spirit level allowing the tilt in the x and y directions to be determined. It also allows an offset to be provided allowing for a more accurate 'zero' position to be used. As the sensor moves the x and y tilt are read and used to indicate the current orientation.

The MotionSensor includes routines to correctly calibrate the MPU6050 which is essential for accurate operation. These are stored in EEPROM by the StrawberryString library for use when next started.

 

Level Indication

 

The RGB LEDs are used to indicate the current level in the following way. Four of the LEDs are arranged in two pairs indicating the tilt in the X and the Y direction.

 

When approximately level the LED hue converges on green, but as the tilt increases the hue changes proportional to the tilt, one of each pair increasing, one decreasing. This gives a visual indication using Hue.

 

When level the LED brightness is the same on the LEDs. As tilt increases the brightness quickly changes, one of each pair increasing and one decreasing. This gives a visual indication using brightness.

 

When precisely level the LEDs change to pink. This gives a very clear indication when you have found your target slope.

 

Releveling

 

Most spirit levels work for horizontal or vertical surfaces. This spirit level can work for arbitrary slopes by using it's releveling feature. When the level is waved back and forth it triggers a releveling routine which takes the current slope as the target slope. It then continues to operate measuring against this new target slope.

 

Releveling Gesture Detection

 

The releveling request gesture is detected from the angular speed of the level following these steps:

 

-Starting from low speed

 

-Rising to high speed

 

-Falling to low speed

 

-Returning to high speed

 

-Falling to low speed

 

-Completing within 2000ms

 

The MotionSensor provides the rotation delta between samples which allows the rotational speed to be indicated. Thresholds for low and high speed were selected by experimentation. A state machine was implemented to manage the detection process using the timer functionality in the SweetMaker framework.

 

Releveling Process

 

Once a relevel request gesture has been detected the releveling process starts. This comprises of the following steps:

 

-Flash the LEDs PURPLE for 10 seconds to allow time for the spirit level to be held in position.

 

-If the spirit level is at rest record the current orientation and use this as an offset and then flash the LEDs green to indicate success.

 

-If the spirit level is not at rest flash the LEDs red to indicate failure.

 

-Flash the LEDs PURPLE to indicate completion.

 

The StrawberryString and MotionSensor support the use of offset rotations and auto-levelling. The StrawberryString also stores this in EEPROM so it is used next time the device starts.

Schematics

 

StrawberryString Schematic

projectImage

Code

 

Spirit Level Sketch

C/C++

CODE
/*******************************************************************************
SpiritLevel.ino - A simple Spirit Level using StrawberryString

Copyright(C) 2022 Howard James May

This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.If not, see <http://www.gnu.org/licenses/>.

Contact me at sweet.maker@outlook.com

*******************************************************************************/
#include <Wire.h>
#include <SweetMaker.h>
#include <MotionSensor.h>
#include "StrawberryString.h"

using namespace SweetMaker;

StrawberryString strStr;

/*
 * These events are generated and handled by this sketch.
 */
static const uint16_t EVENT_AUTO_LEVEL_REQUEST = IEventHandler::USER + 0;
static const uint16_t EVENT_AUTO_LEVEL_COMPLETE = IEventHandler::USER + 1;
static const uint16_t RESET_AUTO_LEVEL_DETECTION = IEventHandler::USER + 2;

/*
 * The LEDS are arranged to show direction of 'lean'
 */
#define LED_CONTROL       (0)
#define LED_TILT_Y_UP     (1)
#define LED_TILT_X_UP     (2)
#define LED_TILT_Y_DOWN   (3)
#define LED_TILT_X_DOWN   (4)

/*
 * Some colours used by the skecth
 */
#define HUE_RED (0x00)
#define HUE_GREEN (0x50)
#define HUE_PINK (0xD0)
#define HUE_PURPLE (0xE0)

/*
 * Function Prototypes used by the sketch
 */
void eventMarshaller(uint16_t eventId, uint8_t src, uint16_t eventInfo);
void detectAutoLevelRequests(uint16_t eventId, uint8_t eventRef, uint16_t eventInfo);
void performAutoLevel(uint16_t eventId, uint8_t eventRef, uint16_t eventInfo);
void actAsSpiritLevel(uint16_t eventId, uint8_t eventRef, uint16_t eventInfo);
void handleSerialInput(void);


/*
 * The spirit level moves between two main states, acting as a spirit level
 * and auto-level where it sets it's 'level' position
 */
enum RunState {
  RUN_STATE_IDLE = 0,
  RUN_STATE_SPIRIT_LEVEL = 1,
  RUN_STATE_AUTO_LEVEL = 2
} runState = RUN_STATE_IDLE;

/*
 * Responsible for configuring and initialising
 *    - strStr - our StrawberryString
 *    - breathingSigGen - our breath Signal Generator
 *    - The Serial interface
 *    - our HSV pixels
 */
void setup()
{
  /* Start Serial at a speed (Baud rate) of 112500 Bytes per second */
  Serial.begin(112500);

  strStr.configEventHandlerCallback(eventMarshaller);
  strStr.init();

  /*
  * Flush Serial 
  */
  while (Serial.available())
    Serial.read();

  Serial.println("Welcome to SpiritLevel");
  runState = RUN_STATE_SPIRIT_LEVEL;
}


/*
 * Main loop - runs forever. This uses the SweetMaker framework and so little happens here
 *
 * The SweetMaker framework is called via 'strStr.update()' this will generate events
 * which are marshalled to 'myEventHandler'.
 *
 */
void loop()
{

  /*
   * We handle some user commands for calibration of the motionSensor
   */
  if (Serial.available())
    handleSerialInput();

  /*
   * This updates the underlying StrawberryString
   */
  strStr.update();
}

/*
 * eventMarshaller - this callback function is called by the SweetMaker framework / StrawberryString
 *                   and notifys us when various events have occured. We then choose how to handle them.
 *                   some events have been generated by our own code.
 */
void eventMarshaller(uint16_t eventId, uint8_t eventRef, uint16_t eventInfo)
{
  /*
   * Start by updating the runState if needed.
   */
  switch (runState) {
  case RUN_STATE_SPIRIT_LEVEL:
  {
    if (eventId == EVENT_AUTO_LEVEL_REQUEST) {
      runState = RUN_STATE_AUTO_LEVEL;
    }
  }
  break;

  case RUN_STATE_AUTO_LEVEL:
  {
    if (eventId == EVENT_AUTO_LEVEL_COMPLETE) {
      runState = RUN_STATE_SPIRIT_LEVEL;
    }
  }
  break;
  }

  /*
   * The autoRun State Machine runs continually so we give it 
   * sight of events
   */
  detectAutoLevelRequests(eventId, eventRef, eventInfo);
  
  /*
   * Now depending on the runState act differently
   */
  switch (runState) {
  case RUN_STATE_SPIRIT_LEVEL:
    actAsSpiritLevel(eventId, eventRef, eventInfo);
    break;

  case RUN_STATE_AUTO_LEVEL:
    performAutoLevel(eventId, eventRef, eventInfo);
    break;
  }

  /*
   * Some events we handle here independent of runState
   */
  switch (eventId) {
  case MotionSensor::MOTION_SENSOR_INIT_ERROR: // This sometimes happens ... best restart
    Serial.println("MOTION_SENSOR_INIT_ERROR: ");
    break;

  case MotionSensor::MOTION_SENSOR_READY:
    Serial.println("MOTION_SENSOR_READY: ");
    break;

  case MotionSensor::MOTION_SENSOR_RUNTIME_ERROR: // shouldn't happen
    Serial.println("Motion Sensor Error");
    break;
  }
}


/*
 * detectAutoLevelRequests - auto leveling is requested by swinging the spiritLevel
 *                           back and forth. This is detected by monitoring the 
 *                           StrawberryString angular speed and looking for
 *                           it crossing our high detection threshold, low
 *                           detection threshold and then low detection threshold
 *                           within a time threshold.
 *
 *                           Once detected an event is generated to indicate an AutoLevel request
 */

enum DetectionState {
  DS_IDLE = 0,
  DS_RESTING_START = 1,
  DS_FIRST_SWING = 2,
  DS_FIRST_REST = 3,
  DS_SECOND_SWING = 4,
  DS_REQUEST_DETECTED = 5
}detectionState = DS_IDLE;

void detectAutoLevelRequests(uint16_t eventId, uint8_t eventRef, uint16_t eventInfo)
{
  static uint8_t autoDetectState = DS_IDLE;
  static Timer detectionTimeoutTimer;

  const uint32_t swing_threshold_high = 1000000;
  const uint32_t swing_threshold_low = 50000;
  const uint16_t timeout_duration_ms = 2000;

  /*
   * In the event of timer expiry we restart this state machine
   */
  if (eventId == TimerTickMngt::TIMER_EXPIRED)  {
    detectionState = DS_IDLE;
    return;
  }

  /*
   * This is the only event we are interested in
   */
  if (eventId != MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY)
    return;

  /*
   * Calculate the angular speed thus:
   */
  RotationQuaternion_16384* rqd = &strStr.motionSensor.rotQuatDelta;
  uint32_t ang_vel = (uint32_t)rqd->x * (uint32_t)rqd->x + (uint32_t)rqd->y * (uint32_t)rqd->y + (uint32_t)rqd->z * (uint32_t)rqd->z;

//  Serial.print(ang_vel);  Serial.print("\t");  Serial.println(detectionState * 100000);

  switch (detectionState)
  {
  case DS_IDLE:
  {
    if (ang_vel < swing_threshold_low) {
      detectionState = DS_RESTING_START;
    }
  }
  break;

  case DS_RESTING_START:
  {
    /*
     * Once we cross the low speed threshold we start the timer.
     */
    if ((ang_vel > swing_threshold_low) &&
      !detectionTimeoutTimer.isRunning())
      detectionTimeoutTimer.startTimer(timeout_duration_ms,0);

    if (ang_vel > swing_threshold_high) {
      detectionState = DS_FIRST_SWING;
    }
  }
  break;

  case DS_FIRST_SWING:
  {
    if (ang_vel < swing_threshold_low) {
      detectionState = DS_FIRST_REST;
    }
  }
  break;

  case DS_FIRST_REST:
  {
    if (ang_vel > swing_threshold_high) {
      detectionState = DS_SECOND_SWING;
    }
  }
  break;

  case DS_SECOND_SWING:
  {
    if (ang_vel < swing_threshold_low)
    {
      /*
       * An auto-level request has been detected - raise event
       * we also start a timer to prevent a new request happening 
       * in the next ten seconds
       */
      detectionState = DS_REQUEST_DETECTED;
      eventMarshaller(EVENT_AUTO_LEVEL_REQUEST, 0, 0);
      detectionTimeoutTimer.stopTimer();
      detectionTimeoutTimer.startTimer(10000, 0);
    }
  }
  break;

  }
}


/*
 * performAutoLevel - this runs through the auto level routine
 *        1) Flash PURPLE for 10 seconds during which the spirit level
 *           should be held still in position 
 *        2a) If the level is stationary update offset and flash green
 *        2b) If the level is no-stationary flash red and don't update  
 *        3) Flash Purple to indicate completion
 *        4) Raise EVENT_AUTO_LEVEL_COMPLETE event
 */
enum AutoLevelState {
  AL_IDLE = 0,
  AL_STARTING = 1,
  AL_SUCCESS = 2,
  AL_FAILURE = 3,
  AL_COMPLETING = 4
}autoLevelState = AL_IDLE;

void performAutoLevel(uint16_t eventId, uint8_t eventRef, uint16_t eventInfo)
{
  static SigGen mySigGen(sineWave255, NUM_SAM(sineWave255), 300, 20);
  static ColourHSV myColourHSV[StrawberryString::num_lights];
  /*
   * Hue/Saturation/Value controll of LEDs
   */

  switch (autoLevelState)
  {
  case AL_IDLE: {
    if (eventId == EVENT_AUTO_LEVEL_REQUEST)
    {
      mySigGen.configPeriod_ms(200);
      mySigGen.start(50);
      for (int i = 0; i < 5; i++) {
        myColourHSV[i].hue = HUE_PURPLE;
        myColourHSV[i].saturation = 0xff;
        myColourHSV[i].value = 0;
      }
      autoLevelState = AL_STARTING;
    }
  }
  break; // AL_IDLE

  case AL_STARTING:
  {
    for (int i = 0; i < 5; i++) {
      myColourHSV[i].value = mySigGen.readValue();
    }

    if (eventId == SigGen::SIG_GEN_FINISHED) {
      RotationQuaternion_16384* rqd = &strStr.motionSensor.rotQuatDelta;
      uint32_t ang_vel = (uint32_t)rqd->x * (uint32_t)rqd->x + (uint32_t)rqd->y * (uint32_t)rqd->y + (uint32_t)rqd->z * (uint32_t)rqd->z;

      if (ang_vel > 1000) {
        autoLevelState = AL_FAILURE;
        for (int i = 0; i < 5; i++) {
          myColourHSV[i].hue = HUE_RED;
        }
        mySigGen.configPeriod_ms(100);
        mySigGen.start(50);
      }
      else {
        Serial.println("Performing Offset");
        autoLevelState = AL_SUCCESS;
        strStr.configOffsetRotation();
        Serial.println(strStr.motionSensor.rotQuat.getSinRotX());
        Serial.println(strStr.motionSensor.rotQuat.getSinRotY());
        for (int i = 0; i < 5; i++) {
          myColourHSV[i].hue = HUE_GREEN;
        }
        mySigGen.configPeriod_ms(500);
        mySigGen.start(4);
      }
    }
  }
  break; // AL_STARTING

  case AL_FAILURE:
  case AL_SUCCESS:
  {
    for (int i = 0; i < 5; i++) {
      myColourHSV[i].value = mySigGen.readValue();
    }

    if (eventId == SigGen::SIG_GEN_FINISHED) {
      autoLevelState = AL_COMPLETING;
      for (int i = 0; i < 5; i++) {
        myColourHSV[i].hue = HUE_PURPLE;
      }
      mySigGen.configPeriod_ms(1000);
      mySigGen.start(1);
    }

  }
  break; // AL_SUCCESS AL_FAILURE

  case AL_COMPLETING:
  {
    for (int i = 0; i < 5; i++) {
      myColourHSV[i].value = mySigGen.readValue();
    }

    if (eventId == SigGen::SIG_GEN_FINISHED) {
      autoLevelState = AL_IDLE;
      eventMarshaller(EVENT_AUTO_LEVEL_COMPLETE, 0, 0);
    }
  }
  break; // AL_COMPLETING

  }

/*
 * Convert from HSV to RGB so the driver can update the LEDs
 */
  for (uint8_t i = 0; i < StrawberryString::num_lights; i++) {
    ColourConverter::ConvertToRGB(myColourHSV + i, strStr.ledStrip + i);
  }

}

/*
 * actAsSpiritLevel: - This looks at the StrawberryString Motion sensor and
 *                     based on the rotation updates the LEDs
 *                     
 *                     It indicates levelness in three ways
 *                     1) The HUE is offset +/- from GREEN
 *                     2) The brightness is offset +/-
 *                     3) When very level LEDs turn PINK
 */
void actAsSpiritLevel(uint16_t eventId, uint8_t eventRef, uint16_t eventInfo)
{
  /*
 * Hue/Saturation/Value controll of LEDs
 */
  ColourHSV myColourHSV[StrawberryString::num_lights];

  if (eventId != MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY)
    return;

  int16_t sinTilt_x = strStr.motionSensor.rotQuat.getSinRotY();
  int16_t sinTilt_y = strStr.motionSensor.rotQuat.getSinRotX();

  Serial.print(sinTilt_x);  Serial.print("\t");  Serial.println(sinTilt_y);

  int16_t x_hue_adjust = Quaternion_16384::asr(sinTilt_x, 6);
  int16_t y_hue_adjust = Quaternion_16384::asr(sinTilt_y, 6);

  if (x_hue_adjust > 0x50)
    x_hue_adjust = 0x50;
  if (x_hue_adjust < -0x50)
    x_hue_adjust = -0x50;

  if (y_hue_adjust > 0x50)
    y_hue_adjust = 0x50;
  if (y_hue_adjust < -0x50)
    y_hue_adjust = -0x50;

  int16_t x_value_adjust = sinTilt_x;
  int16_t y_value_adjust = sinTilt_y;

  if (x_value_adjust > 0x40)
    x_value_adjust = 0x40;
  if (x_value_adjust < -0x40)
    x_value_adjust = -0x40;

  if (y_value_adjust > 0x40)
    y_value_adjust = 0x40;
  if (y_value_adjust < -0x40)
    y_value_adjust = -0x40;

  myColourHSV[LED_CONTROL].hue = 0x40;
  myColourHSV[LED_TILT_X_UP].hue = HUE_GREEN + x_hue_adjust;
  myColourHSV[LED_TILT_Y_UP].hue = HUE_GREEN + y_hue_adjust;
  myColourHSV[LED_TILT_X_DOWN].hue = HUE_GREEN - x_hue_adjust;
  myColourHSV[LED_TILT_Y_DOWN].hue = HUE_GREEN - y_hue_adjust;

  myColourHSV[LED_TILT_X_UP].value = 0x60 - x_value_adjust;
  myColourHSV[LED_TILT_Y_UP].value = 0x60 - y_value_adjust;
  myColourHSV[LED_TILT_X_DOWN].value = 0x60 + x_value_adjust;
  myColourHSV[LED_TILT_Y_DOWN].value = 0x60 + y_value_adjust;

  myColourHSV[LED_TILT_X_UP].saturation = 0xff;
  myColourHSV[LED_TILT_Y_UP].saturation = 0xff;
  myColourHSV[LED_TILT_X_DOWN].saturation = 0xff;
  myColourHSV[LED_TILT_Y_DOWN].saturation = 0xff;

  if (abs(sinTilt_x) < 10)
  {
    myColourHSV[LED_TILT_X_UP].hue = HUE_PINK + x_hue_adjust;
    myColourHSV[LED_TILT_X_DOWN].hue = HUE_PINK - x_hue_adjust;
  }
  if (abs(sinTilt_y) < 10)
  {
    myColourHSV[LED_TILT_Y_UP].hue = HUE_PINK + y_hue_adjust;
    myColourHSV[LED_TILT_Y_DOWN].hue = HUE_PINK - y_hue_adjust;
  }

  /*
 * Convert from HSV to RGB so the driver can update the LEDs
 */
  for (uint8_t i = 0; i < StrawberryString::num_lights; i++) {
    ColourConverter::ConvertToRGB(myColourHSV + i, strStr.ledStrip + i);
  }

}


/*
 * handleSerialInput - Handles user commands on the serial interface
 */
void handleSerialInput(void)
{
  /*
   * Grab character
   */
  char c = Serial.read();
  switch (c) {

  case 'c':
    /* Run MPU6050 callibration - make sure sensor is flat and still*/
    Serial.println("Starting Self Cal");
    strStr.recalibrateMotionSensor();
    Serial.println("Writing to EEPROM");
    break;
 
  }
}

StrawberryString Repository

icon StrawberryString-main.zip 27KB Download(0)

SweetMaker MotionSensor

icon MotionSensor-main.zip 92KB Download(0)

SweetMaker Core

icon core-main.zip 87KB Download(0)

The article was first published in hackster, March 10 2022

cr: https://www.hackster.io/SweetMaker/electronic-spirit-level-8845ab

author: SweetMaker

License
All Rights
Reserved
licensBg
0