Electronic Spirit Level

1/*******************************************************************************
2SpiritLevel.ino
3  - A simple Spirit Level using StrawberryString
4
5Copyright(C) 2022 Howard James
6  May
7
8This program is free software : you can redistribute it and / or modify
9it
10  under the terms of the GNU General Public License as published by
11the Free Software
12  Foundation, either version 3 of the License, or
13(at your option) any later version.
14
15This
16  program is distributed in the hope that it will be useful,
17but WITHOUT ANY WARRANTY;
18  without even the implied warranty of
19MERCHANTABILITY or FITNESS FOR A PARTICULAR
20  PURPOSE.See the
21GNU General Public License for more details.
22
23You should
24  have received a copy of the GNU General Public License
25along with this program.If
26  not, see <http://www.gnu.org/licenses/>.
27
28Contact me at sweet.maker@outlook.com
29
30*******************************************************************************/
31#include
32  <Wire.h>
33#include <SweetMaker.h>
34#include <MotionSensor.h>
35#include "StrawberryString.h"
36
37using
38  namespace SweetMaker;
39
40StrawberryString strStr;
41
42/*
43 * These events
44  are generated and handled by this sketch.
45 */
46static const uint16_t EVENT_AUTO_LEVEL_REQUEST
47  = IEventHandler::USER + 0;
48static const uint16_t EVENT_AUTO_LEVEL_COMPLETE =
49  IEventHandler::USER + 1;
50static const uint16_t RESET_AUTO_LEVEL_DETECTION = IEventHandler::USER
51  + 2;
52
53/*
54 * The LEDS are arranged to show direction of 'lean'
55 */
56#define
57  LED_CONTROL       (0)
58#define LED_TILT_Y_UP     (1)
59#define LED_TILT_X_UP
60     (2)
61#define LED_TILT_Y_DOWN   (3)
62#define LED_TILT_X_DOWN   (4)
63
64/*
65
66  * Some colours used by the skecth
67 */
68#define HUE_RED (0x00)
69#define HUE_GREEN
70  (0x50)
71#define HUE_PINK (0xD0)
72#define HUE_PURPLE (0xE0)
73
74/*
75 * Function
76  Prototypes used by the sketch
77 */
78void eventMarshaller(uint16_t eventId, uint8_t
79  src, uint16_t eventInfo);
80void detectAutoLevelRequests(uint16_t eventId, uint8_t
81  eventRef, uint16_t eventInfo);
82void performAutoLevel(uint16_t eventId, uint8_t
83  eventRef, uint16_t eventInfo);
84void actAsSpiritLevel(uint16_t eventId, uint8_t
85  eventRef, uint16_t eventInfo);
86void handleSerialInput(void);
87
88
89/*
90
91  * The spirit level moves between two main states, acting as a spirit level
92 *
93  and auto-level where it sets it's 'level' position
94 */
95enum RunState {
96
97  RUN_STATE_IDLE = 0,
98  RUN_STATE_SPIRIT_LEVEL = 1,
99  RUN_STATE_AUTO_LEVEL
100  = 2
101} runState = RUN_STATE_IDLE;
102
103/*
104 * Responsible for configuring
105  and initialising
106 *    - strStr - our StrawberryString
107 *    - breathingSigGen
108  - our breath Signal Generator
109 *    - The Serial interface
110 *    - our HSV
111  pixels
112 */
113void setup()
114{
115  /* Start Serial at a speed (Baud rate) of
116  112500 Bytes per second */
117  Serial.begin(112500);
118
119  strStr.configEventHandlerCallback(eventMarshaller);
120
121  strStr.init();
122
123  /*
124  * Flush Serial 
125  */
126  while (Serial.available())
127
128    Serial.read();
129
130  Serial.println("Welcome to SpiritLevel");
131  runState
132  = RUN_STATE_SPIRIT_LEVEL;
133}
134
135
136/*
137 * Main loop - runs forever. This
138  uses the SweetMaker framework and so little happens here
139 *
140 * The SweetMaker
141  framework is called via 'strStr.update()' this will generate events
142 * which
143  are marshalled to 'myEventHandler'.
144 *
145 */
146void loop()
147{
148
149  /*
150
151   * We handle some user commands for calibration of the motionSensor
152   */
153
154  if (Serial.available())
155    handleSerialInput();
156
157  /*
158   * This updates
159  the underlying StrawberryString
160   */
161  strStr.update();
162}
163
164/*
165
166  * eventMarshaller - this callback function is called by the SweetMaker framework
167  / StrawberryString
168 *                   and notifys us when various events have
169  occured. We then choose how to handle them.
170 *                   some events
171  have been generated by our own code.
172 */
173void eventMarshaller(uint16_t eventId,
174  uint8_t eventRef, uint16_t eventInfo)
175{
176  /*
177   * Start by updating the
178  runState if needed.
179   */
180  switch (runState) {
181  case RUN_STATE_SPIRIT_LEVEL:
182
183  {
184    if (eventId == EVENT_AUTO_LEVEL_REQUEST) {
185      runState = RUN_STATE_AUTO_LEVEL;
186
187    }
188  }
189  break;
190
191  case RUN_STATE_AUTO_LEVEL:
192  {
193    if (eventId
194  == EVENT_AUTO_LEVEL_COMPLETE) {
195      runState = RUN_STATE_SPIRIT_LEVEL;
196
197    }
198  }
199  break;
200  }
201
202  /*
203   * The autoRun State Machine runs
204  continually so we give it 
205   * sight of events
206   */
207  detectAutoLevelRequests(eventId,
208  eventRef, eventInfo);
209  
210  /*
211   * Now depending on the runState act differently
212
213   */
214  switch (runState) {
215  case RUN_STATE_SPIRIT_LEVEL:
216    actAsSpiritLevel(eventId,
217  eventRef, eventInfo);
218    break;
219
220  case RUN_STATE_AUTO_LEVEL:
221    performAutoLevel(eventId,
222  eventRef, eventInfo);
223    break;
224  }
225
226  /*
227   * Some events we handle
228  here independent of runState
229   */
230  switch (eventId) {
231  case MotionSensor::MOTION_SENSOR_INIT_ERROR:
232  // This sometimes happens ... best restart
233    Serial.println("MOTION_SENSOR_INIT_ERROR:
234  ");
235    break;
236
237  case MotionSensor::MOTION_SENSOR_READY:
238    Serial.println("MOTION_SENSOR_READY:
239  ");
240    break;
241
242  case MotionSensor::MOTION_SENSOR_RUNTIME_ERROR: // shouldn't
243  happen
244    Serial.println("Motion Sensor Error");
245    break;
246  }
247}
248
249
250/*
251
252  * detectAutoLevelRequests - auto leveling is requested by swinging the spiritLevel
253
254  *                           back and forth. This is detected by monitoring the 
255
256  *                           StrawberryString angular speed and looking for
257 *
258                           it crossing our high detection threshold, low
259 *                           detection
260  threshold and then low detection threshold
261 *                           within
262  a time threshold.
263 *
264 *                           Once detected an event is
265  generated to indicate an AutoLevel request
266 */
267
268enum DetectionState {
269
270  DS_IDLE = 0,
271  DS_RESTING_START = 1,
272  DS_FIRST_SWING = 2,
273  DS_FIRST_REST
274  = 3,
275  DS_SECOND_SWING = 4,
276  DS_REQUEST_DETECTED = 5
277}detectionState =
278  DS_IDLE;
279
280void detectAutoLevelRequests(uint16_t eventId, uint8_t eventRef,
281  uint16_t eventInfo)
282{
283  static uint8_t autoDetectState = DS_IDLE;
284  static
285  Timer detectionTimeoutTimer;
286
287  const uint32_t swing_threshold_high = 1000000;
288
289  const uint32_t swing_threshold_low = 50000;
290  const uint16_t timeout_duration_ms
291  = 2000;
292
293  /*
294   * In the event of timer expiry we restart this state machine
295
296   */
297  if (eventId == TimerTickMngt::TIMER_EXPIRED)  {
298    detectionState
299  = DS_IDLE;
300    return;
301  }
302
303  /*
304   * This is the only event we are
305  interested in
306   */
307  if (eventId != MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY)
308
309    return;
310
311  /*
312   * Calculate the angular speed thus:
313   */
314  RotationQuaternion_16384*
315  rqd = &strStr.motionSensor.rotQuatDelta;
316  uint32_t ang_vel = (uint32_t)rqd->x
317  * (uint32_t)rqd->x + (uint32_t)rqd->y * (uint32_t)rqd->y + (uint32_t)rqd->z * (uint32_t)rqd->z;
318
319//
320  Serial.print(ang_vel);  Serial.print("\	");  Serial.println(detectionState
321  * 100000);
322
323  switch (detectionState)
324  {
325  case DS_IDLE:
326  {
327
328    if (ang_vel < swing_threshold_low) {
329      detectionState = DS_RESTING_START;
330
331    }
332  }
333  break;
334
335  case DS_RESTING_START:
336  {
337    /*
338     *
339  Once we cross the low speed threshold we start the timer.
340     */
341    if ((ang_vel
342  > swing_threshold_low) &&
343      !detectionTimeoutTimer.isRunning())
344      detectionTimeoutTimer.startTimer(timeout_duration_ms,0);
345
346
347    if (ang_vel > swing_threshold_high) {
348      detectionState = DS_FIRST_SWING;
349
350    }
351  }
352  break;
353
354  case DS_FIRST_SWING:
355  {
356    if (ang_vel
357  < swing_threshold_low) {
358      detectionState = DS_FIRST_REST;
359    }
360  }
361
362  break;
363
364  case DS_FIRST_REST:
365  {
366    if (ang_vel > swing_threshold_high)
367  {
368      detectionState = DS_SECOND_SWING;
369    }
370  }
371  break;
372
373
374  case DS_SECOND_SWING:
375  {
376    if (ang_vel < swing_threshold_low)
377    {
378
379      /*
380       * An auto-level request has been detected - raise event
381       *
382  we also start a timer to prevent a new request happening 
383       * in the next
384  ten seconds
385       */
386      detectionState = DS_REQUEST_DETECTED;
387      eventMarshaller(EVENT_AUTO_LEVEL_REQUEST,
388  0, 0);
389      detectionTimeoutTimer.stopTimer();
390      detectionTimeoutTimer.startTimer(10000,
391  0);
392    }
393  }
394  break;
395
396  }
397}
398
399
400/*
401 * performAutoLevel
402  - this runs through the auto level routine
403 *        1) Flash PURPLE for 10 seconds
404  during which the spirit level
405 *           should be held still in position 
406
407  *        2a) If the level is stationary update offset and flash green
408 *        2b)
409  If the level is no-stationary flash red and don't update  
410 *        3) Flash
411  Purple to indicate completion
412 *        4) Raise EVENT_AUTO_LEVEL_COMPLETE event
413
414  */
415enum AutoLevelState {
416  AL_IDLE = 0,
417  AL_STARTING = 1,
418  AL_SUCCESS
419  = 2,
420  AL_FAILURE = 3,
421  AL_COMPLETING = 4
422}autoLevelState = AL_IDLE;
423
424void
425  performAutoLevel(uint16_t eventId, uint8_t eventRef, uint16_t eventInfo)
426{
427
428  static SigGen mySigGen(sineWave255, NUM_SAM(sineWave255), 300, 20);
429  static
430  ColourHSV myColourHSV[StrawberryString::num_lights];
431  /*
432   * Hue/Saturation/Value
433  controll of LEDs
434   */
435
436  switch (autoLevelState)
437  {
438  case AL_IDLE:
439  {
440    if (eventId == EVENT_AUTO_LEVEL_REQUEST)
441    {
442      mySigGen.configPeriod_ms(200);
443
444      mySigGen.start(50);
445      for (int i = 0; i < 5; i++) {
446        myColourHSV[i].hue
447  = HUE_PURPLE;
448        myColourHSV[i].saturation = 0xff;
449        myColourHSV[i].value
450  = 0;
451      }
452      autoLevelState = AL_STARTING;
453    }
454  }
455  break;
456  // AL_IDLE
457
458  case AL_STARTING:
459  {
460    for (int i = 0; i < 5; i++)
461  {
462      myColourHSV[i].value = mySigGen.readValue();
463    }
464
465    if (eventId
466  == SigGen::SIG_GEN_FINISHED) {
467      RotationQuaternion_16384* rqd = &strStr.motionSensor.rotQuatDelta;
468
469      uint32_t ang_vel = (uint32_t)rqd->x * (uint32_t)rqd->x + (uint32_t)rqd->y
470  * (uint32_t)rqd->y + (uint32_t)rqd->z * (uint32_t)rqd->z;
471
472      if (ang_vel
473  > 1000) {
474        autoLevelState = AL_FAILURE;
475        for (int i = 0; i <
476  5; i++) {
477          myColourHSV[i].hue = HUE_RED;
478        }
479        mySigGen.configPeriod_ms(100);
480
481        mySigGen.start(50);
482      }
483      else {
484        Serial.println("Performing
485  Offset");
486        autoLevelState = AL_SUCCESS;
487        strStr.configOffsetRotation();
488
489        Serial.println(strStr.motionSensor.rotQuat.getSinRotX());
490        Serial.println(strStr.motionSensor.rotQuat.getSinRotY());
491
492        for (int i = 0; i < 5; i++) {
493          myColourHSV[i].hue = HUE_GREEN;
494
495        }
496        mySigGen.configPeriod_ms(500);
497        mySigGen.start(4);
498
499      }
500    }
501  }
502  break; // AL_STARTING
503
504  case AL_FAILURE:
505
506  case AL_SUCCESS:
507  {
508    for (int i = 0; i < 5; i++) {
509      myColourHSV[i].value
510  = mySigGen.readValue();
511    }
512
513    if (eventId == SigGen::SIG_GEN_FINISHED)
514  {
515      autoLevelState = AL_COMPLETING;
516      for (int i = 0; i < 5; i++)
517  {
518        myColourHSV[i].hue = HUE_PURPLE;
519      }
520      mySigGen.configPeriod_ms(1000);
521
522      mySigGen.start(1);
523    }
524
525  }
526  break; // AL_SUCCESS AL_FAILURE
527
528
529  case AL_COMPLETING:
530  {
531    for (int i = 0; i < 5; i++) {
532      myColourHSV[i].value
533  = mySigGen.readValue();
534    }
535
536    if (eventId == SigGen::SIG_GEN_FINISHED)
537  {
538      autoLevelState = AL_IDLE;
539      eventMarshaller(EVENT_AUTO_LEVEL_COMPLETE,
540  0, 0);
541    }
542  }
543  break; // AL_COMPLETING
544
545  }
546
547/*
548 * Convert
549  from HSV to RGB so the driver can update the LEDs
550 */
551  for (uint8_t i = 0;
552  i < StrawberryString::num_lights; i++) {
553    ColourConverter::ConvertToRGB(myColourHSV
554  + i, strStr.ledStrip + i);
555  }
556
557}
558
559/*
560 * actAsSpiritLevel: - This
561  looks at the StrawberryString Motion sensor and
562 *                     based
563  on the rotation updates the LEDs
564 *                     
565 *                     It
566  indicates levelness in three ways
567 *                     1) The HUE is offset
568  +/- from GREEN
569 *                     2) The brightness is offset +/-
570 *                     3)
571  When very level LEDs turn PINK
572 */
573void actAsSpiritLevel(uint16_t eventId,
574  uint8_t eventRef, uint16_t eventInfo)
575{
576  /*
577 * Hue/Saturation/Value controll
578  of LEDs
579 */
580  ColourHSV myColourHSV[StrawberryString::num_lights];
581
582
583  if (eventId != MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY)
584    return;
585
586
587  int16_t sinTilt_x = strStr.motionSensor.rotQuat.getSinRotY();
588  int16_t sinTilt_y
589  = strStr.motionSensor.rotQuat.getSinRotX();
590
591  Serial.print(sinTilt_x);  Serial.print("\	");
592  Serial.println(sinTilt_y);
593
594  int16_t x_hue_adjust = Quaternion_16384::asr(sinTilt_x,
595  6);
596  int16_t y_hue_adjust = Quaternion_16384::asr(sinTilt_y, 6);
597
598  if
599  (x_hue_adjust > 0x50)
600    x_hue_adjust = 0x50;
601  if (x_hue_adjust < -0x50)
602
603    x_hue_adjust = -0x50;
604
605  if (y_hue_adjust > 0x50)
606    y_hue_adjust
607  = 0x50;
608  if (y_hue_adjust < -0x50)
609    y_hue_adjust = -0x50;
610
611  int16_t
612  x_value_adjust = sinTilt_x;
613  int16_t y_value_adjust = sinTilt_y;
614
615  if
616  (x_value_adjust > 0x40)
617    x_value_adjust = 0x40;
618  if (x_value_adjust <
619  -0x40)
620    x_value_adjust = -0x40;
621
622  if (y_value_adjust > 0x40)
623    y_value_adjust
624  = 0x40;
625  if (y_value_adjust < -0x40)
626    y_value_adjust = -0x40;
627
628
629  myColourHSV[LED_CONTROL].hue = 0x40;
630  myColourHSV[LED_TILT_X_UP].hue = HUE_GREEN
631  + x_hue_adjust;
632  myColourHSV[LED_TILT_Y_UP].hue = HUE_GREEN + y_hue_adjust;
633
634  myColourHSV[LED_TILT_X_DOWN].hue = HUE_GREEN - x_hue_adjust;
635  myColourHSV[LED_TILT_Y_DOWN].hue
636  = HUE_GREEN - y_hue_adjust;
637
638  myColourHSV[LED_TILT_X_UP].value = 0x60 - x_value_adjust;
639
640  myColourHSV[LED_TILT_Y_UP].value = 0x60 - y_value_adjust;
641  myColourHSV[LED_TILT_X_DOWN].value
642  = 0x60 + x_value_adjust;
643  myColourHSV[LED_TILT_Y_DOWN].value = 0x60 + y_value_adjust;
644
645
646  myColourHSV[LED_TILT_X_UP].saturation = 0xff;
647  myColourHSV[LED_TILT_Y_UP].saturation
648  = 0xff;
649  myColourHSV[LED_TILT_X_DOWN].saturation = 0xff;
650  myColourHSV[LED_TILT_Y_DOWN].saturation
651  = 0xff;
652
653  if (abs(sinTilt_x) < 10)
654  {
655    myColourHSV[LED_TILT_X_UP].hue
656  = HUE_PINK + x_hue_adjust;
657    myColourHSV[LED_TILT_X_DOWN].hue = HUE_PINK -
658  x_hue_adjust;
659  }
660  if (abs(sinTilt_y) < 10)
661  {
662    myColourHSV[LED_TILT_Y_UP].hue
663  = HUE_PINK + y_hue_adjust;
664    myColourHSV[LED_TILT_Y_DOWN].hue = HUE_PINK -
665  y_hue_adjust;
666  }
667
668  /*
669 * Convert from HSV to RGB so the driver can
670  update the LEDs
671 */
672  for (uint8_t i = 0; i < StrawberryString::num_lights;
673  i++) {
674    ColourConverter::ConvertToRGB(myColourHSV + i, strStr.ledStrip + i);
675
676  }
677
678}
679
680
681/*
682 * handleSerialInput - Handles user commands on the
683  serial interface
684 */
685void handleSerialInput(void)
686{
687  /*
688   * Grab
689  character
690   */
691  char c = Serial.read();
692  switch (c) {
693
694  case
695  'c':
696    /* Run MPU6050 callibration - make sure sensor is flat and still*/
697
698    Serial.println("Starting Self Cal");
699    strStr.recalibrateMotionSensor();
700
701    Serial.println("Writing to EEPROM");
702    break;
703 
704  }
705}

1/*******************************************************************************
2SpiritLevel.ino  - A simple Spirit Level using StrawberryString
3
4Copyright(C) 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 <SweetMaker.h>
24#include <MotionSensor.h>
25#include "StrawberryString.h"
26
27using  namespace SweetMaker;
28
29StrawberryString strStr;
30
31/*
32 * These events  are generated and handled by this sketch.
33 */
34static const uint16_t EVENT_AUTO_LEVEL_REQUEST  = IEventHandler::USER + 0;
35static const uint16_t EVENT_AUTO_LEVEL_COMPLETE =  IEventHandler::USER + 1;
36static const uint16_t RESET_AUTO_LEVEL_DETECTION = IEventHandler::USER  + 2;
37
38/*
39 * The LEDS are arranged to show direction of 'lean'
40 */
41#define  LED_CONTROL       (0)
42#define LED_TILT_Y_UP     (1)
43#define LED_TILT_X_UP     (2)
44#define LED_TILT_Y_DOWN   (3)
45#define LED_TILT_X_DOWN   (4)
46
47/*
48  * Some colours used by the skecth
49 */
50#define HUE_RED (0x00)
51#define HUE_GREEN  (0x50)
52#define HUE_PINK (0xD0)
53#define HUE_PURPLE (0xE0)
54
55/*
56 * Function  Prototypes used by the sketch
57 */
58void eventMarshaller(uint16_t eventId, uint8_t  src, uint16_t eventInfo);
59void detectAutoLevelRequests(uint16_t eventId, uint8_t  eventRef, uint16_t eventInfo);
60void performAutoLevel(uint16_t eventId, uint8_t  eventRef, uint16_t eventInfo);
61void actAsSpiritLevel(uint16_t eventId, uint8_t  eventRef, uint16_t eventInfo);
62void handleSerialInput(void);
63
64
65/*
66  * The spirit level moves between two main states, acting as a spirit level
67 *  and auto-level where it sets it's 'level' position
68 */
69enum RunState {
70  RUN_STATE_IDLE = 0,
71  RUN_STATE_SPIRIT_LEVEL = 1,
72  RUN_STATE_AUTO_LEVEL  = 2
73} runState = RUN_STATE_IDLE;
74
75/*
76 * Responsible for configuring  and initialising
77 *    - strStr - our StrawberryString
78 *    - breathingSigGen  - our breath Signal Generator
79 *    - The Serial interface
80 *    - our HSV  pixels
81 */
82void setup()
83{
84  /* Start Serial at a speed (Baud rate) of  112500 Bytes per second */
85  Serial.begin(112500);
86
87  strStr.configEventHandlerCallback(eventMarshaller);
88  strStr.init();
89
90  /*
91  * Flush Serial 
92  */
93  while (Serial.available())
94    Serial.read();
95
96  Serial.println("Welcome to SpiritLevel");
97  runState  = RUN_STATE_SPIRIT_LEVEL;
98}
99
100
101/*
102 * Main loop - runs forever. This  uses the SweetMaker framework and so little happens here
103 *
104 * The SweetMaker  framework is called via 'strStr.update()' this will generate events
105 * which  are marshalled to 'myEventHandler'.
106 *
107 */
108void loop()
109{
110
111  /*
112   * We handle some user commands for calibration of the motionSensor
113   */
114  if (Serial.available())
115    handleSerialInput();
116
117  /*
118   * This updates  the underlying StrawberryString
119   */
120  strStr.update();
121}
122
123/*
124  * eventMarshaller - this callback function is called by the SweetMaker framework  / StrawberryString
125 *                   and notifys us when various events have  occured. We then choose how to handle them.
126 *                   some events  have been generated by our own code.
127 */
128void eventMarshaller(uint16_t eventId,  uint8_t eventRef, uint16_t eventInfo)
129{
130  /*
131   * Start by updating the  runState if needed.
132   */
133  switch (runState) {
134  case RUN_STATE_SPIRIT_LEVEL:
135  {
136    if (eventId == EVENT_AUTO_LEVEL_REQUEST) {
137      runState = RUN_STATE_AUTO_LEVEL;
138    }
139  }
140  break;
141
142  case RUN_STATE_AUTO_LEVEL:
143  {
144    if (eventId  == EVENT_AUTO_LEVEL_COMPLETE) {
145      runState = RUN_STATE_SPIRIT_LEVEL;
146    }
147  }
148  break;
149  }
150
151  /*
152   * The autoRun State Machine runs  continually so we give it 
153   * sight of events
154   */
155  detectAutoLevelRequests(eventId,  eventRef, eventInfo);
156  
157  /*
158   * Now depending on the runState act differently
159   */
160  switch (runState) {
161  case RUN_STATE_SPIRIT_LEVEL:
162    actAsSpiritLevel(eventId,  eventRef, eventInfo);
163    break;
164
165  case RUN_STATE_AUTO_LEVEL:
166    performAutoLevel(eventId,  eventRef, eventInfo);
167    break;
168  }
169
170  /*
171   * Some events we handle  here independent of runState
172   */
173  switch (eventId) {
174  case MotionSensor::MOTION_SENSOR_INIT_ERROR:  // This sometimes happens ... best restart
175    Serial.println("MOTION_SENSOR_INIT_ERROR:  ");
176    break;
177
178  case MotionSensor::MOTION_SENSOR_READY:
179    Serial.println("MOTION_SENSOR_READY:  ");
180    break;
181
182  case MotionSensor::MOTION_SENSOR_RUNTIME_ERROR: // shouldn't  happen
183    Serial.println("Motion Sensor Error");
184    break;
185  }
186}
187
188
189/*
190  * detectAutoLevelRequests - auto leveling is requested by swinging the spiritLevel
191  *                           back and forth. This is detected by monitoring the 
192  *                           StrawberryString angular speed and looking for
193 *                           it crossing our high detection threshold, low
194 *                           detection  threshold and then low detection threshold
195 *                           within  a time threshold.
196 *
197 *                           Once detected an event is  generated to indicate an AutoLevel request
198 */
199
200enum DetectionState {
201  DS_IDLE = 0,
202  DS_RESTING_START = 1,
203  DS_FIRST_SWING = 2,
204  DS_FIRST_REST  = 3,
205  DS_SECOND_SWING = 4,
206  DS_REQUEST_DETECTED = 5
207}detectionState =  DS_IDLE;
208
209void detectAutoLevelRequests(uint16_t eventId, uint8_t eventRef,  uint16_t eventInfo)
210{
211  static uint8_t autoDetectState = DS_IDLE;
212  static  Timer detectionTimeoutTimer;
213
214  const uint32_t swing_threshold_high = 1000000;
215  const uint32_t swing_threshold_low = 50000;
216  const uint16_t timeout_duration_ms  = 2000;
217
218  /*
219   * In the event of timer expiry we restart this state machine
220   */
221  if (eventId == TimerTickMngt::TIMER_EXPIRED)  {
222    detectionState  = DS_IDLE;
223    return;
224  }
225
226  /*
227   * This is the only event we are  interested in
228   */
229  if (eventId != MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY)
230    return;
231
232  /*
233   * Calculate the angular speed thus:
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//  Serial.print(ang_vel);  Serial.print("\	");  Serial.println(detectionState  * 100000);
239
240  switch (detectionState)
241  {
242  case DS_IDLE:
243  {
244    if (ang_vel < swing_threshold_low) {
245      detectionState = DS_RESTING_START;
246    }
247  }
248  break;
249
250  case DS_RESTING_START:
251  {
252    /*
253     *  Once we cross the low speed threshold we start the timer.
254     */
255    if ((ang_vel  > swing_threshold_low) &&
256      !detectionTimeoutTimer.isRunning())
257      detectionTimeoutTimer.startTimer(timeout_duration_ms,0);
258
259    if (ang_vel > swing_threshold_high) {
260      detectionState = DS_FIRST_SWING;
261    }
262  }
263  break;
264
265  case DS_FIRST_SWING:
266  {
267    if (ang_vel  < swing_threshold_low) {
268      detectionState = DS_FIRST_REST;
269    }
270  }
271  break;
272
273  case DS_FIRST_REST:
274  {
275    if (ang_vel > swing_threshold_high)  {
276      detectionState = DS_SECOND_SWING;
277    }
278  }
279  break;
280
281  case DS_SECOND_SWING:
282  {
283    if (ang_vel < swing_threshold_low)
284    {
285      /*
286       * An auto-level request has been detected - raise event
287       *  we also start a timer to prevent a new request happening 
288       * in the next  ten seconds
289       */
290      detectionState = DS_REQUEST_DETECTED;
291      eventMarshaller(EVENT_AUTO_LEVEL_REQUEST,  0, 0);
292      detectionTimeoutTimer.stopTimer();
293      detectionTimeoutTimer.startTimer(10000,  0);
294    }
295  }
296  break;
297
298  }
299}
300
301
302/*
303 * performAutoLevel  - this runs through the auto level routine
304 *        1) Flash PURPLE for 10 seconds  during which the spirit level
305 *           should be held still in position 
306  *        2a) If the level is stationary update offset and flash green
307 *        2b)  If the level is no-stationary flash red and don't update  
308 *        3) Flash  Purple to indicate completion
309 *        4) Raise EVENT_AUTO_LEVEL_COMPLETE event
310  */
311enum AutoLevelState {
312  AL_IDLE = 0,
313  AL_STARTING = 1,
314  AL_SUCCESS  = 2,
315  AL_FAILURE = 3,
316  AL_COMPLETING = 4
317}autoLevelState = AL_IDLE;
318
319void  performAutoLevel(uint16_t eventId, uint8_t eventRef, uint16_t eventInfo)
320{
321  static SigGen mySigGen(sineWave255, NUM_SAM(sineWave255), 300, 20);
322  static  ColourHSV myColourHSV[StrawberryString::num_lights];
323  /*
324   * Hue/Saturation/Value  controll of LEDs
325   */
326
327  switch (autoLevelState)
328  {
329  case AL_IDLE:  {
330    if (eventId == EVENT_AUTO_LEVEL_REQUEST)
331    {
332      mySigGen.configPeriod_ms(200);
333      mySigGen.start(50);
334      for (int i = 0; i < 5; i++) {
335        myColourHSV[i].hue  = HUE_PURPLE;
336        myColourHSV[i].saturation = 0xff;
337        myColourHSV[i].value  = 0;
338      }
339      autoLevelState = AL_STARTING;
340    }
341  }
342  break;  // AL_IDLE
343
344  case AL_STARTING:
345  {
346    for (int i = 0; i < 5; i++)  {
347      myColourHSV[i].value = mySigGen.readValue();
348    }
349
350    if (eventId  == SigGen::SIG_GEN_FINISHED) {
351      RotationQuaternion_16384* rqd = &strStr.motionSensor.rotQuatDelta;
352      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;
353
354      if (ang_vel  > 1000) {
355        autoLevelState = AL_FAILURE;
356        for (int i = 0; i <  5; i++) {
357          myColourHSV[i].hue = HUE_RED;
358        }
359        mySigGen.configPeriod_ms(100);
360        mySigGen.start(50);
361      }
362      else {
363        Serial.println("Performing  Offset");
364        autoLevelState = AL_SUCCESS;
365        strStr.configOffsetRotation();
366        Serial.println(strStr.motionSensor.rotQuat.getSinRotX());
367        Serial.println(strStr.motionSensor.rotQuat.getSinRotY());
368        for (int i = 0; i < 5; i++) {
369          myColourHSV[i].hue = HUE_GREEN;
370        }
371        mySigGen.configPeriod_ms(500);
372        mySigGen.start(4);
373      }
374    }
375  }
376  break; // AL_STARTING
377
378  case AL_FAILURE:
379  case AL_SUCCESS:
380  {
381    for (int i = 0; i < 5; i++) {
382      myColourHSV[i].value  = mySigGen.readValue();
383    }
384
385    if (eventId == SigGen::SIG_GEN_FINISHED)  {
386      autoLevelState = AL_COMPLETING;
387      for (int i = 0; i < 5; i++)  {
388        myColourHSV[i].hue = HUE_PURPLE;
389      }
390      mySigGen.configPeriod_ms(1000);
391      mySigGen.start(1);
392    }
393
394  }
395  break; // AL_SUCCESS AL_FAILURE
396
397  case AL_COMPLETING:
398  {
399    for (int i = 0; i < 5; i++) {
400      myColourHSV[i].value  = mySigGen.readValue();
401    }
402
403    if (eventId == SigGen::SIG_GEN_FINISHED)  {
404      autoLevelState = AL_IDLE;
405      eventMarshaller(EVENT_AUTO_LEVEL_COMPLETE,  0, 0);
406    }
407  }
408  break; // AL_COMPLETING
409
410  }
411
412/*
413 * Convert  from HSV to RGB so the driver can update the LEDs
414 */
415  for (uint8_t i = 0;  i < StrawberryString::num_lights; i++) {
416    ColourConverter::ConvertToRGB(myColourHSV  + i, strStr.ledStrip + i);
417  }
418
419}
420
421/*
422 * actAsSpiritLevel: - This  looks at the StrawberryString Motion sensor and
423 *                     based  on the rotation updates the LEDs
424 *                     
425 *                     It  indicates levelness in three ways
426 *                     1) The HUE is offset  +/- from GREEN
427 *                     2) The brightness is offset +/-
428 *                     3)  When very level LEDs turn PINK
429 */
430void actAsSpiritLevel(uint16_t eventId,  uint8_t eventRef, uint16_t eventInfo)
431{
432  /*
433 * Hue/Saturation/Value controll  of LEDs
434 */
435  ColourHSV myColourHSV[StrawberryString::num_lights];
436
437  if (eventId != MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY)
438    return;
439
440  int16_t sinTilt_x = strStr.motionSensor.rotQuat.getSinRotY();
441  int16_t sinTilt_y  = strStr.motionSensor.rotQuat.getSinRotX();
442
443  Serial.print(sinTilt_x);  Serial.print("\	");  Serial.println(sinTilt_y);
444
445  int16_t x_hue_adjust = Quaternion_16384::asr(sinTilt_x,  6);
446  int16_t y_hue_adjust = Quaternion_16384::asr(sinTilt_y, 6);
447
448  if  (x_hue_adjust > 0x50)
449    x_hue_adjust = 0x50;
450  if (x_hue_adjust < -0x50)
451    x_hue_adjust = -0x50;
452
453  if (y_hue_adjust > 0x50)
454    y_hue_adjust  = 0x50;
455  if (y_hue_adjust < -0x50)
456    y_hue_adjust = -0x50;
457
458  int16_t  x_value_adjust = sinTilt_x;
459  int16_t y_value_adjust = sinTilt_y;
460
461  if  (x_value_adjust > 0x40)
462    x_value_adjust = 0x40;
463  if (x_value_adjust <  -0x40)
464    x_value_adjust = -0x40;
465
466  if (y_value_adjust > 0x40)
467    y_value_adjust  = 0x40;
468  if (y_value_adjust < -0x40)
469    y_value_adjust = -0x40;
470
471  myColourHSV[LED_CONTROL].hue = 0x40;
472  myColourHSV[LED_TILT_X_UP].hue = HUE_GREEN  + x_hue_adjust;
473  myColourHSV[LED_TILT_Y_UP].hue = HUE_GREEN + y_hue_adjust;
474  myColourHSV[LED_TILT_X_DOWN].hue = HUE_GREEN - x_hue_adjust;
475  myColourHSV[LED_TILT_Y_DOWN].hue  = HUE_GREEN - y_hue_adjust;
476
477  myColourHSV[LED_TILT_X_UP].value = 0x60 - x_value_adjust;
478  myColourHSV[LED_TILT_Y_UP].value = 0x60 - y_value_adjust;
479  myColourHSV[LED_TILT_X_DOWN].value  = 0x60 + x_value_adjust;
480  myColourHSV[LED_TILT_Y_DOWN].value = 0x60 + y_value_adjust;
481
482  myColourHSV[LED_TILT_X_UP].saturation = 0xff;
483  myColourHSV[LED_TILT_Y_UP].saturation  = 0xff;
484  myColourHSV[LED_TILT_X_DOWN].saturation = 0xff;
485  myColourHSV[LED_TILT_Y_DOWN].saturation  = 0xff;
486
487  if (abs(sinTilt_x) < 10)
488  {
489    myColourHSV[LED_TILT_X_UP].hue  = HUE_PINK + x_hue_adjust;
490    myColourHSV[LED_TILT_X_DOWN].hue = HUE_PINK -  x_hue_adjust;
491  }
492  if (abs(sinTilt_y) < 10)
493  {
494    myColourHSV[LED_TILT_Y_UP].hue  = HUE_PINK + y_hue_adjust;
495    myColourHSV[LED_TILT_Y_DOWN].hue = HUE_PINK -  y_hue_adjust;
496  }
497
498  /*
499 * Convert from HSV to RGB so the driver can  update the LEDs
500 */
501  for (uint8_t i = 0; i < StrawberryString::num_lights;  i++) {
502    ColourConverter::ConvertToRGB(myColourHSV + i, strStr.ledStrip + i);
503  }
504
505}
506
507
508/*
509 * handleSerialInput - Handles user commands on the  serial interface
510 */
511void handleSerialInput(void)
512{
513  /*
514   * Grab  character
515   */
516  char c = Serial.read();
517  switch (c) {
518
519  case  'c':
520    /* Run MPU6050 callibration - make sure sensor is flat and still*/
521    Serial.println("Starting Self Cal");
522    strStr.recalibrateMotionSensor();
523    Serial.println("Writing to EEPROM");
524    break;
525 
526  }
527}