Electronic Spirit Level

/*******************************************************************************
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;
 
  }
}