SweetMaker StrawberryString

/*******************************************************************************
LightHat.ino - A Motion controlled Light Hat using StrawberryString

Copyright(C) 2017-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 <EEPROM.h>
#include <SweetMaker.h>
#include <MotionSensor.h>
#include "StrawberryString.h"
#include "EepromUtility.h"
#include "SigLib.h"

using namespace SweetMaker;

StrawberryString strStr;

/*
 * These events are generated by this sketch and also handled by it.
 */
static const uint16_t EVENT_CALLIBRATE = IEventHandler::USER + 0;
static const uint16_t EVENT_SET_ROT_OFF = IEventHandler::USER + 1;
static const uint16_t EVENT_PRINT_POSITION = IEventHandler::USER + 2;

void myEventHandler(uint16_t eventId, uint8_t src, uint16_t eventInfo);

/*
 * Signal generator creating a gentle breating effect
 */
SigGen breatingSigGen;

/*
 * Hue/Saturation/Value controll of LEDs
 */
ColourHSV myColourHSV[StrawberryString::num_lights];

/*
 * Flag to enable some diagnostics
 */
bool diagnosticsOn = false;

/*
 * Function Prototypes
 */
void processMotionSensorReading(void);
void handleSerialInput(void);


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

  breatingSigGen.configSamples(breathingWave, NUM_SAM(breathingWave), 2000, 0); // period of 2000ms
  breatingSigGen.start();  // start and repeat forever

	strStr.configEventHandlerCallback(myEventHandler);
	strStr.init();

	for(int i=0;i<strStr.num_lights;i++)
	  myColourHSV[i].setColour(123, 255, 255);

	/*
	* Flush Serial and start timer looking for user input in first 5 seconds
	*/
	while (Serial.available())
		Serial.read();

	Serial.println("LightCap");

}


/*
 * Main loop - runs forever. This uses the SweetMaker framework and so 
 * not much happens here other than updating the StrawberryString LEDs to whatever
 * value the HSV pixels have been set to.
 *
 * The SweetMaker framework is called via 'strStr.update()' this will generate events
 * which are marshalled to 'myEventHandler'.
 *
 */
void loop()
{
  /*
   * Performance Monitor records loop time intervals - these are printed out
   * when requested on the Serial Input.
   */
  PerfMon::getPerfMon()->intervalStop();
	PerfMon::getPerfMon()->intervalStart();

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

  /*
   * We handle some user commands for calibration and setup
   */
	if (Serial.available())
    handleSerialInput();

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

/*
 * myEventHandler - 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 in response to Serial input.
 */
void myEventHandler(uint16_t eventId, uint8_t eventRef, uint16_t eventInfo)
{
	switch (eventId)
	{
	case EVENT_CALLIBRATE:	{
    /*
     * A request to callibrate the StrawberryString Motion Sensor. Each individual MPU6050 motion sensor
     * requires calibration. The StrawberryString stores the callibration values in EEPROM so 
     * calibration is a one time task. 
     *
     * When running Calibrating the motion sensor should be level and the MPU6050 chip topmost. Don't move
     * the Strawberry String until calibration is complete .. about 10 seconds.
     */
		Serial.println("Starting Self Cal");
		strStr.recalibrateMotionSensor();
		Serial.println("Writing to EEPROM");
		break;
	}

  case EVENT_PRINT_POSITION: {
    /*
     * Print out the current position / orientation
     */
    Serial.println("Print Position");
    int16_t roll = strStr.motionSensor.rotQuat.getSinRotX();
    int16_t pitch = strStr.motionSensor.rotQuat.getSinRotY();
    int16_t yaw_16384 = strStr.motionSensor.rotQuat.getSinRotZ();
    
    Serial.print(roll); Serial.print(" ");
    Serial.print(pitch); Serial.print(" ");
    Serial.print(yaw_16384); Serial.println();
    strStr.motionSensor.rotQuat.printQ();

    break;
  }

  case TimerTickMngt::TIMER_TICK_100MS: // Generated ten times a second
    if (diagnosticsOn) {
      // place Diagnostic Code here
    }
    break;
  
  case TimerTickMngt::TIMER_TICK_S: // This event is called every second
    // myEventHandler(EVENT_PRINT_POSITION, 0, 0);
	  break;

	case TimerTickMngt::TIMER_TICK_10S: // This event is called every ten seconds
		break;

	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: 
		Serial.println("Motion Sensor Error");
		break;

	case MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY:
    /*
     * This is where we do the main work 
     */
    processMotionSensorReading();
    break;


    /*
     * Other events which we aren't using
     */
	case SigGen::SIG_GEN_STOPPED: // A Signal Generator has been stopped
	case TimerTickMngt::TIMER_TICK_UPDATE: // Generated every time fizzyMint is updated - could be every 500us (micro seconds) e.g. less than a millisecond
	case TimerTickMngt::TIMER_FREQ_GEN: // Generated a certain number of times a seconds
	case TimerTickMngt::TIMER_EXPIRED: // A timer has expired - eventInfo from timerId
	case SigGen::SIG_GEN_STARTED: // A Signal Generator has been started
	case SigGen::SIG_GEN_FINISHED: // A Signal Generator has finished 
		break;

	}
}


/*
 * processMotionSensorReading: - This looks at the StrawberryString Motion sensor and 
 *              based on the rotation updates the LEDs
 */
void processMotionSensorReading(void)
{
  /*
   * Start by calculating the angular velocity by looking at the Rotation Quaternion Delta.
   * The Square of x, y and z gives the square of the square of sin(dtheta/2) where dTheta is the change
   * in angle over the last 10ms (presuming the motion sensor is running at 100Hz). 
   * We use this as an indication of the angular velocity. 
   */
  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;

  /*
   * Scale this down so it's a uint8_t
   */
  ang_vel = (ang_vel >> 8);
  if (ang_vel > 255)
    ang_vel = 255;

  /*
   * Get the tilt and scale
   */
  int16_t tilt_forward = strStr.motionSensor.rotQuat.getSinRotY();
  tilt_forward = tilt_forward >> 8;

  /*
   * Get the tilt_side, scale and take absolute value
   */
  int16_t tilt_side = strStr.motionSensor.rotQuat.getSinRotX();
  tilt_side = tilt_side >> 8;
  tilt_side = abs(tilt_side);

  /*
   * Get the updated 'breathing' value
   * And use it only if tilting to one side
   */
  uint32_t breathing = breatingSigGen.readValue();
  breathing = (breathing * tilt_side) >> 6;

  if (breathing > 200)
    breathing = 200;

  /*
   * Using ang_vel_sticky to add a slower decay to the ang_vel
   */
  static uint32_t ang_vel_sticky = 0;
  if (ang_vel > ang_vel_sticky)
    ang_vel_sticky = ang_vel;
  else if (ang_vel_sticky > 4)
    ang_vel_sticky -= 4;
  else
    ang_vel_sticky = 0;


  /*
   * Set the hue to 200 (purple) and vary it depending on the tilt forward
   */
  uint8_t hue = 200 + tilt_forward;

  /*
   * Set saturation to max but reduce when moving and when 'breathing'
   */
  int16_t saturation = 255 - (ang_vel_sticky /2) - breathing;
  if (saturation < 0)
    saturation = 0;

  /*
   * Set the brightness to 156 but increase when moving.
   */
  uint16_t value = ang_vel_sticky + 156;
  if (value > 255)
    value = 255;
  
  /*
   * Now update the HSV pixels
   */
  for (int i = 0; i < strStr.num_lights; i++) {
    myColourHSV[i].hue = hue;
    myColourHSV[i].saturation = (uint8_t)saturation;
    myColourHSV[i].value = (uint8_t)value;
  }

}


/*
 * handleSerialInput - Handles user commands on the serial interface
 */
void handleSerialInput(void)
{
  /*
   * Default offset rotation for the SweetMaker 
   */
  static int rz = -26; // rotation about Z axis

  char c = Serial.read();
  switch (c) {
  case 's':
    /* Print Out Performance Stats and then reset them */
    PerfMon::getPerfMon()->intervalStop();
    PerfMon::getPerfMon()->print();
    PerfMon::getPerfMon()->reset();
    PerfMon::getPerfMon()->intervalStart();
    break;

  case 'c':
    /* Run MPU6050 callibration - make sure sensor is flat */
    myEventHandler(EVENT_CALLIBRATE, 0, 0);
    break;

  case 'p':
    /* Print Current Orientation */
    myEventHandler(EVENT_PRINT_POSITION, 0, 0);
    break;

  case 'd':
    if(diagnosticsOn)
      diagnosticsOn = false;
    else
      diagnosticsOn = true;
    break;


  case ',':
  {
    /* Decrement Rotation Offset about Z axis and auto-level*/
    rz--;
    Serial.println(rz);
    strStr.configOffsetRotation(rz);
    myEventHandler(EVENT_PRINT_POSITION, 0, 0);
    break;
  }
  case '.':
  {
    /* Increment Rotation Offset about Z axis and auto-level*/
    rz++;
    Serial.println(rz);
    strStr.configOffsetRotation(rz);
    myEventHandler(EVENT_PRINT_POSITION, 0, 0);
    break;
  }
  }
}