SweetMaker StrawberryString

1/*******************************************************************************
2LightHat.ino  - A Motion controlled Light Hat using StrawberryString
3
4Copyright(C) 2017-2022  Howard James May
5
6This program is free software : you can redistribute it  and / or modify
7it under the terms of the GNU General Public License as published  by
8the Free Software Foundation, either version 3 of the License, or
9(at your  option) any later version.
10
11This program is distributed in the hope that it  will be useful,
12but WITHOUT ANY WARRANTY; without even the implied warranty of
13MERCHANTABILITY  or FITNESS FOR A PARTICULAR PURPOSE.See the
14GNU General Public License for more  details.
15
16You should have received a copy of the GNU General Public License
17along  with this program.If not, see <http://www.gnu.org/licenses/>.
18
19Contact me  at sweet.maker@outlook.com
20
21*******************************************************************************/
22#include  <Wire.h>
23#include <EEPROM.h>
24#include <SweetMaker.h>
25#include <MotionSensor.h>
26#include  "StrawberryString.h"
27#include "EepromUtility.h"
28#include "SigLib.h"
29
30using  namespace SweetMaker;
31
32StrawberryString strStr;
33
34/*
35 * These events  are generated by this sketch and also handled by it.
36 */
37static const uint16_t  EVENT_CALLIBRATE = IEventHandler::USER + 0;
38static const uint16_t EVENT_SET_ROT_OFF  = IEventHandler::USER + 1;
39static const uint16_t EVENT_PRINT_POSITION = IEventHandler::USER  + 2;
40
41void myEventHandler(uint16_t eventId, uint8_t src, uint16_t eventInfo);
42
43/*
44  * Signal generator creating a gentle breating effect
45 */
46SigGen breatingSigGen;
47
48/*
49  * Hue/Saturation/Value controll of LEDs
50 */
51ColourHSV myColourHSV[StrawberryString::num_lights];
52
53/*
54  * Flag to enable some diagnostics
55 */
56bool diagnosticsOn = false;
57
58/*
59  * Function Prototypes
60 */
61void processMotionSensorReading(void);
62void handleSerialInput(void);
63
64
65/*
66  * Responsible for configuring and initialising 
67 *    - strStr - our StrawberryString
68  *    - breathingSigGen - our breath Signal Generator
69 *    - The Serial interface
70  *    - our HSV pixels
71 */
72void setup()
73{
74	/* Start Serial at a speed  (Baud rate) of 112500 Bytes per second */
75	Serial.begin(112500);
76
77  breatingSigGen.configSamples(breathingWave,  NUM_SAM(breathingWave), 2000, 0); // period of 2000ms
78  breatingSigGen.start();  // start and repeat forever
79
80	strStr.configEventHandlerCallback(myEventHandler);
81	strStr.init();
82
83	for(int  i=0;i<strStr.num_lights;i++)
84	  myColourHSV[i].setColour(123, 255, 255);
85
86	/*
87	*  Flush Serial and start timer looking for user input in first 5 seconds
88	*/
89	while  (Serial.available())
90		Serial.read();
91
92	Serial.println("LightCap");
93
94}
95
96
97/*
98  * Main loop - runs forever. This uses the SweetMaker framework and so 
99 * not  much happens here other than updating the StrawberryString LEDs to whatever
100  * value the HSV pixels have been set to.
101 *
102 * The SweetMaker framework is  called via 'strStr.update()' this will generate events
103 * which are marshalled  to 'myEventHandler'.
104 *
105 */
106void loop()
107{
108  /*
109   * Performance  Monitor records loop time intervals - these are printed out
110   * when requested  on the Serial Input.
111   */
112  PerfMon::getPerfMon()->intervalStop();
113	PerfMon::getPerfMon()->intervalStart();
114
115  /*
116   * Convert from HSV to RGB so the driver can update the LEDs
117   */
118	for  (uint8_t i = 0; i < StrawberryString::num_lights; i++)	{
119		ColourConverter::ConvertToRGB(myColourHSV  + i, strStr.ledStrip + i);
120	}
121
122  /*
123   * We handle some user commands  for calibration and setup
124   */
125	if (Serial.available())
126    handleSerialInput();
127
128  /*
129   * This updates the underlying StrawberryString
130   */
131	strStr.update();
132}
133
134/*
135  * myEventHandler - this callback function is called by the SweetMaker framework  / StrawberryString
136 *                  and notifys us when various events have  occured. We then choose how to handle them.
137 *                  some events have  been generated by our own code in response to Serial input.
138 */
139void myEventHandler(uint16_t  eventId, uint8_t eventRef, uint16_t eventInfo)
140{
141	switch (eventId)
142	{
143	case  EVENT_CALLIBRATE:	{
144    /*
145     * A request to callibrate the StrawberryString  Motion Sensor. Each individual MPU6050 motion sensor
146     * requires calibration.  The StrawberryString stores the callibration values in EEPROM so 
147     * calibration  is a one time task. 
148     *
149     * When running Calibrating the motion sensor  should be level and the MPU6050 chip topmost. Don't move
150     * the Strawberry  String until calibration is complete .. about 10 seconds.
151     */
152		Serial.println("Starting  Self Cal");
153		strStr.recalibrateMotionSensor();
154		Serial.println("Writing  to EEPROM");
155		break;
156	}
157
158  case EVENT_PRINT_POSITION: {
159    /*
160     * Print out the current position / orientation
161     */
162    Serial.println("Print  Position");
163    int16_t roll = strStr.motionSensor.rotQuat.getSinRotX();
164    int16_t pitch = strStr.motionSensor.rotQuat.getSinRotY();
165    int16_t yaw_16384  = strStr.motionSensor.rotQuat.getSinRotZ();
166    
167    Serial.print(roll); Serial.print("  ");
168    Serial.print(pitch); Serial.print(" ");
169    Serial.print(yaw_16384);  Serial.println();
170    strStr.motionSensor.rotQuat.printQ();
171
172    break;
173  }
174
175  case TimerTickMngt::TIMER_TICK_100MS: // Generated ten times a second
176    if (diagnosticsOn) {
177      // place Diagnostic Code here
178    }
179    break;
180  
181  case TimerTickMngt::TIMER_TICK_S: // This event is called every second
182    // myEventHandler(EVENT_PRINT_POSITION, 0, 0);
183	  break;
184
185	case TimerTickMngt::TIMER_TICK_10S:  // This event is called every ten seconds
186		break;
187
188	case MotionSensor::MOTION_SENSOR_INIT_ERROR:  // This sometimes happens ... best restart
189		Serial.println("MOTION_SENSOR_INIT_ERROR:  ");
190		break;
191
192	case MotionSensor::MOTION_SENSOR_READY:
193		Serial.println("MOTION_SENSOR_READY:  ");
194		break;
195
196	case MotionSensor::MOTION_SENSOR_RUNTIME_ERROR: 
197		Serial.println("Motion  Sensor Error");
198		break;
199
200	case MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY:
201    /*
202     * This is where we do the main work 
203     */
204    processMotionSensorReading();
205    break;
206
207
208    /*
209     * Other events which we aren't using
210     */
211	case  SigGen::SIG_GEN_STOPPED: // A Signal Generator has been stopped
212	case TimerTickMngt::TIMER_TICK_UPDATE:  // Generated every time fizzyMint is updated - could be every 500us (micro seconds)  e.g. less than a millisecond
213	case TimerTickMngt::TIMER_FREQ_GEN: // Generated  a certain number of times a seconds
214	case TimerTickMngt::TIMER_EXPIRED: // A  timer has expired - eventInfo from timerId
215	case SigGen::SIG_GEN_STARTED: //  A Signal Generator has been started
216	case SigGen::SIG_GEN_FINISHED: // A Signal  Generator has finished 
217		break;
218
219	}
220}
221
222
223/*
224 * processMotionSensorReading:  - This looks at the StrawberryString Motion sensor and 
225 *              based  on the rotation updates the LEDs
226 */
227void processMotionSensorReading(void)
228{
229  /*
230   * Start by calculating the angular velocity by looking at the Rotation  Quaternion Delta.
231   * The Square of x, y and z gives the square of the square  of sin(dtheta/2) where dTheta is the change
232   * in angle over the last 10ms  (presuming the motion sensor is running at 100Hz). 
233   * We use this as an indication  of the angular velocity. 
234   */
235  RotationQuaternion_16384* rqd = &strStr.motionSensor.rotQuatDelta;  
236  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;
237
238  /*
239   * Scale  this down so it's a uint8_t
240   */
241  ang_vel = (ang_vel >> 8);
242  if (ang_vel  > 255)
243    ang_vel = 255;
244
245  /*
246   * Get the tilt and scale
247   */
248  int16_t tilt_forward = strStr.motionSensor.rotQuat.getSinRotY();
249  tilt_forward  = tilt_forward >> 8;
250
251  /*
252   * Get the tilt_side, scale and take absolute  value
253   */
254  int16_t tilt_side = strStr.motionSensor.rotQuat.getSinRotX();
255  tilt_side = tilt_side >> 8;
256  tilt_side = abs(tilt_side);
257
258  /*
259   *  Get the updated 'breathing' value
260   * And use it only if tilting to one side
261   */
262  uint32_t breathing = breatingSigGen.readValue();
263  breathing = (breathing  * tilt_side) >> 6;
264
265  if (breathing > 200)
266    breathing = 200;
267
268  /*
269   * Using ang_vel_sticky to add a slower decay to the ang_vel
270   */
271  static uint32_t ang_vel_sticky = 0;
272  if (ang_vel > ang_vel_sticky)
273    ang_vel_sticky  = ang_vel;
274  else if (ang_vel_sticky > 4)
275    ang_vel_sticky -= 4;
276  else
277    ang_vel_sticky = 0;
278
279
280  /*
281   * Set the hue to 200 (purple) and  vary it depending on the tilt forward
282   */
283  uint8_t hue = 200 + tilt_forward;
284
285  /*
286   * Set saturation to max but reduce when moving and when 'breathing'
287   */
288  int16_t saturation = 255 - (ang_vel_sticky /2) - breathing;
289  if (saturation  < 0)
290    saturation = 0;
291
292  /*
293   * Set the brightness to 156 but increase  when moving.
294   */
295  uint16_t value = ang_vel_sticky + 156;
296  if (value  > 255)
297    value = 255;
298  
299  /*
300   * Now update the HSV pixels
301   */
302  for (int i = 0; i < strStr.num_lights; i++) {
303    myColourHSV[i].hue = hue;
304    myColourHSV[i].saturation = (uint8_t)saturation;
305    myColourHSV[i].value  = (uint8_t)value;
306  }
307
308}
309
310
311/*
312 * handleSerialInput - Handles  user commands on the serial interface
313 */
314void handleSerialInput(void)
315{
316  /*
317   * Default offset rotation for the SweetMaker 
318   */
319  static int  rz = -26; // rotation about Z axis
320
321  char c = Serial.read();
322  switch  (c) {
323  case 's':
324    /* Print Out Performance Stats and then reset them */
325    PerfMon::getPerfMon()->intervalStop();
326    PerfMon::getPerfMon()->print();
327    PerfMon::getPerfMon()->reset();
328    PerfMon::getPerfMon()->intervalStart();
329    break;
330
331  case 'c':
332    /* Run MPU6050 callibration - make sure sensor  is flat */
333    myEventHandler(EVENT_CALLIBRATE, 0, 0);
334    break;
335
336  case 'p':
337    /* Print Current Orientation */
338    myEventHandler(EVENT_PRINT_POSITION,  0, 0);
339    break;
340
341  case 'd':
342    if(diagnosticsOn)
343      diagnosticsOn  = false;
344    else
345      diagnosticsOn = true;
346    break;
347
348
349  case  ',':
350  {
351    /* Decrement Rotation Offset about Z axis and auto-level*/
352    rz--;
353    Serial.println(rz);
354    strStr.configOffsetRotation(rz);
355    myEventHandler(EVENT_PRINT_POSITION, 0, 0);
356    break;
357  }
358  case '.':
359  {
360    /* Increment Rotation Offset about Z axis and auto-level*/
361    rz++;
362    Serial.println(rz);
363    strStr.configOffsetRotation(rz);
364    myEventHandler(EVENT_PRINT_POSITION,  0, 0);
365    break;
366  }
367  }
368}