SweetMaker
Published © GPL3+

Electronic Spirit Level

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

IntermediateFull instructions provided1,191
Electronic Spirit Level

Things used in this project

Story

Read more

Schematics

StrawberryString Schematic

Code

Spirit Level Sketch

C/C++
/*******************************************************************************
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

SweetMaker MotionSensor

SweetMaker Core

Credits

SweetMaker

SweetMaker

3 projects • 0 followers

Comments