StateChart for behavior of FlippityFlappity in presentation. Behavior here is predictable and deterministic.

Dependencies:   MMA8451Q Multi_WS2811_Demo TSI mbed

Files at this revision

API Documentation at this revision

Comitter:
ryanfeng
Date:
Mon Apr 06 20:29:33 2015 +0000
Commit message:
Demo Code for presentation

Changed in this revision

MMA8451Q.lib Show annotated file Show diff for this revision Revisions of this file
Multi_WS2811.lib Show annotated file Show diff for this revision Revisions of this file
TSI.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
mpr121.cpp Show annotated file Show diff for this revision Revisions of this file
mpr121.h Show annotated file Show diff for this revision Revisions of this file
statechart.cpp Show annotated file Show diff for this revision Revisions of this file
statechart.h Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 8f759cbfc3c9 MMA8451Q.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MMA8451Q.lib	Mon Apr 06 20:29:33 2015 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/emilmont/code/MMA8451Q/#c4d879a39775
diff -r 000000000000 -r 8f759cbfc3c9 Multi_WS2811.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Multi_WS2811.lib	Mon Apr 06 20:29:33 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/ryanfeng/code/Multi_WS2811_Demo/#c4123ca9b02c
diff -r 000000000000 -r 8f759cbfc3c9 TSI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TSI.lib	Mon Apr 06 20:29:33 2015 +0000
@@ -0,0 +1,1 @@
+https://mbed.org/users/mbed_official/code/TSI/#1a60ef257879
diff -r 000000000000 -r 8f759cbfc3c9 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 06 20:29:33 2015 +0000
@@ -0,0 +1,107 @@
+#include "mbed.h"
+#include "string"
+#include "list"
+#include "WS2811.h"
+#include "Colors.h"
+#include "TSISensor.h"
+#include "MMA8451Q.h"
+#include "mpr121.h"
+#include "statechart.h"
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+
+/*-----------------------------------------------------------------------*/
+
+// Neopixel Setup
+unsigned const nLEDs = MAX_LEDS_PER_STRIP;          // Defined in WS2811.h
+unsigned const DATA_OUT_PIN1 = 2;                   // PTD2
+unsigned const DATA_OUT_PIN2 = 3;                   // PTD3
+
+// Maestro Setup
+Serial maestro(USBTX,USBRX);
+
+// Timer Setup
+Timer timeRunning;
+
+// MPR121 Setup
+InterruptIn interrupt(D9);
+I2C i2c(D14, D15);
+Mpr121 mpr121(&i2c, Mpr121::ADD_VSS); // constructor(i2c object, i2c address of the mpr121)
+int touch_val = 0;
+int prox_val = 0;
+
+// Accelerometer Setup
+MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
+
+// Battery Read Setup
+AnalogIn battery(A1);
+
+/*-----------------------------------------------------------------------*/
+
+void setToProxMode( bool prox ) {
+    
+    if( prox ) {                                                            // Proximity Mode Setup
+    
+        mpr121.setProximityMode(true);
+        unsigned char mode = mpr121.read(ELE_CFG);                          // Get the current mode
+        mpr121.write(ELE_CFG,0x00);                                         // Put the MPR into setup mode
+        mpr121.write(EPROXTTH, 0x00);                                       // Write the new threshold
+        mpr121.write(EPROXRTH, 0x00);
+        mpr121.write(ELE_CFG, mode);                                        //Restore the operating mode
+        
+    } else {                                                                // Touch Mode Setup
+    
+        mpr121.setProximityMode(false);
+        unsigned char badTouch[12]     = {  0x04, 0x04, 0x04,               // Tail-side Body
+                                            0x04, 0x04, 0x04,               // Tail-side End
+                                            0x07, 0x07, 0x07,               // Head-side Body
+                                            0x05, 0x05, 0x05,};             // Head-side End
+    
+        unsigned char sweetRelease[12] = {  0x03, 0x03, 0x03,               // Tail-side Body
+                                            0x03, 0x03, 0x03,               // Tail-side End
+                                            0x02, 0x02, 0x02,               // Head-side Body
+                                            0x02, 0x02, 0x02};              // Head-side End
+        for( int i = 0; i < 12; i++ ) {                             
+            mpr121.setElectrodeThreshold(i,badTouch[i],sweetRelease[i]);    // Reset Touch Sensitivity
+        }
+    }
+}
+
+// Primary Interrupt Function for MPR121
+void fallInterrupt() {
+    touch_val = mpr121.read(0x00);
+    touch_val += mpr121.read(0x01) << 8;
+}
+
+/*-----------------------------------------------------------------------*/
+
+int main() {
+    
+    WS2811 lightStrip1(nLEDs, DATA_OUT_PIN1);
+    WS2811 lightStrip2(nLEDs, DATA_OUT_PIN2);
+    lightStrip1.begin();
+    lightStrip2.begin();
+
+    Statechart statechart;
+
+    // Set to Touch Mode
+    setToProxMode(false);
+    
+    // Start MPR121 Interrupts
+    interrupt.fall(&fallInterrupt);
+    interrupt.mode(PullUp);
+    
+    // Main Loop: Show Neopixels
+    timeRunning.start();
+    float currTime;
+    float xyz[3];
+    
+    while(true) {
+        currTime = timeRunning.read_ms();
+        acc.getAccAllAxis(xyz);
+        
+        statechart.fishStatechart(currTime, touch_val, prox_val, xyz, maestro, lightStrip1, lightStrip2, battery);
+    }
+}
+
+ 
\ No newline at end of file
diff -r 000000000000 -r 8f759cbfc3c9 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Mon Apr 06 20:29:33 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/4fc01daae5a5
\ No newline at end of file
diff -r 000000000000 -r 8f759cbfc3c9 mpr121.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mpr121.cpp	Mon Apr 06 20:29:33 2015 +0000
@@ -0,0 +1,221 @@
+/*
+Copyright (c) 2011 Anthony Buckton (abuckton [at] blackink [dot} net {dot} au)
+ 
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+ 
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+ 
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*/
+
+#include <mbed.h>
+#include <sstream>
+#include <string>
+#include <list>
+
+#include <mpr121.h>
+    
+Mpr121::Mpr121(I2C *i2c, Address i2cAddress)
+{
+    this->i2c = i2c;
+    
+    address = i2cAddress;
+           
+    // Configure the MPR121 settings to default
+    this->configureSettings();
+}
+
+   
+void Mpr121::configureSettings()
+{
+    // Put the MPR into setup mode
+    this->write(ELE_CFG,0x00);
+    
+    // Electrode filters for when data is > baseline
+    unsigned char gtBaseline[] = {
+         0x01,  //MHD_R
+         0x01,  //NHD_R 
+         0x00,  //NCL_R
+         0x00   //FDL_R
+         };
+         
+    writeMany(MHD_R,gtBaseline,4);   
+                 
+     // Electrode filters for when data is < baseline   
+     unsigned char ltBaseline[] = {
+        0x01,   //MHD_F
+        0x01,   //NHD_F
+        0xFF,   //NCL_F
+        0x02    //FDL_F
+        };
+        
+    writeMany(MHD_F,ltBaseline,4);
+        
+    // Electrode touch and release thresholds
+    unsigned char electrodeThresholds[] = {
+        E_THR_T, // Touch Threshhold
+        E_THR_R  // Release Threshold
+        };
+
+    for(int i=0; i<12; i++){
+        int result = writeMany((ELE0_T+(i*2)),electrodeThresholds,2);
+    }   
+
+    // Proximity Settings
+    unsigned char proximitySettings[] = {
+        0xff,   //MHD_Prox_R
+        0xff,   //NHD_Prox_R
+        0x00,   //NCL_Prox_R
+        0x00,   //FDL_Prox_R
+        0x01,   //MHD_Prox_F
+        0x01,   //NHD_Prox_F
+        0xFF,   //NCL_Prox_F
+        0xff,   //FDL_Prox_F
+        0x00,   //NHD_Prox_T
+        0x00,   //NCL_Prox_T
+        0x00    //NFD_Prox_T
+        };
+    writeMany(MHDPROXR,proximitySettings,11);
+
+    unsigned char proxThresh[] = {
+        PROX_THR_T, // Touch Threshold
+        PROX_THR_R  // Release Threshold
+        };
+    writeMany(EPROXTTH,proxThresh,2); 
+       
+    this->write(FIL_CFG,0x04);
+    
+    // Set the electrode config to transition to active mode
+    this->write(ELE_CFG,0x0c);
+}
+
+void Mpr121::setElectrodeThreshold(int electrode, unsigned char touch, unsigned char release){
+    
+    if(electrode > 11) return;
+    
+    // Get the current mode
+    unsigned char mode = this->read(ELE_CFG);
+    
+    // Put the MPR into setup mode
+    this->write(ELE_CFG,0x00);
+    
+    // Write the new threshold
+    this->write((ELE0_T+(electrode*2)), touch);
+    this->write((ELE0_T+(electrode*2)+1), release);
+    
+    //Restore the operating mode
+    this->write(ELE_CFG, mode);
+}
+    
+    
+unsigned char Mpr121::read(int key){
+
+    unsigned char data[2];
+    
+    //Start the command
+    i2c->start();
+
+    // Address the target (Write mode)
+    int ack1= i2c->write(address);
+
+    // Set the register key to read
+    int ack2 = i2c->write(key);
+
+    // Re-start for read of data
+    i2c->start();
+
+    // Re-send the target address in read mode
+    int ack3 = i2c->write(address+1);
+
+    // Read in the result
+    data[0] = i2c->read(0); 
+
+    // Reset the bus        
+    i2c->stop();
+
+    return data[0];
+}
+
+
+int Mpr121::write(int key, unsigned char value){
+    
+    //Start the command
+    i2c->start();
+
+    // Address the target (Write mode)
+    int ack1= i2c->write(address);
+
+    // Set the register key to write
+    int ack2 = i2c->write(key);
+
+    // Read in the result
+    int ack3 = i2c->write(value); 
+
+    // Reset the bus        
+    i2c->stop();
+    
+    return (ack1+ack2+ack3)-3;
+}
+
+
+int Mpr121::writeMany(int start, unsigned char* dataSet, int length){
+    //Start the command
+    i2c->start();
+
+    // Address the target (Write mode)
+    int ack= i2c->write(address);
+    if(ack!=1){
+        return -1;
+    }
+    
+    // Set the register key to write
+    ack = i2c->write(start);
+    if(ack!=1){
+        return -1;
+    }
+
+    // Write the date set
+    int count = 0;
+    while(ack==1 && (count < length)){
+        ack = i2c->write(dataSet[count]);
+        count++;
+    } 
+    // Stop the cmd
+    i2c->stop();
+    
+    return count;
+}
+      
+
+bool Mpr121::getProximityMode(){
+    if(this->read(ELE_CFG) > 0x0c)
+        return true;
+    else
+        return false;
+}
+
+void Mpr121::setProximityMode(bool mode){
+    this->write(ELE_CFG,0x00);
+    if(mode){
+        this->write(ELE_CFG,0x30); //Sense proximity from ALL pads
+    } else {
+        this->write(ELE_CFG,0x0c); //Sense touch, all 12 pads active.
+    }
+}
+
+
+int Mpr121::readTouchData(){
+    return this->read(0x00);
+}
\ No newline at end of file
diff -r 000000000000 -r 8f759cbfc3c9 mpr121.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mpr121.h	Mon Apr 06 20:29:33 2015 +0000
@@ -0,0 +1,157 @@
+/*
+Copyright (c) 2011 Anthony Buckton (abuckton [at] blackink [dot} net {dot} au)
+
+ 
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+ 
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+ 
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+
+   Parts written by Jim Lindblom of Sparkfun
+   Ported to mbed by A.Buckton, Feb 2011
+*/
+
+#ifndef MPR121_H
+#define MPR121_H
+
+//using namespace std;
+
+class Mpr121 
+{
+
+public:
+    // i2c Addresses, bit-shifted
+    enum Address { ADD_VSS = 0xb4,// ADD->VSS = 0x5a <-wiring on Sparkfun board
+                   ADD_VDD = 0xb6,// ADD->VDD = 0x5b
+                   ADD_SCL = 0xb8,// ADD->SDA = 0x5c
+                   ADD_SDA = 0xba // ADD->SCL = 0x5d
+                 };
+
+    // Real initialiser, takes the i2c address of the device.
+    Mpr121(I2C *i2c, Address i2cAddress);
+    
+    bool getProximityMode();
+    
+    void setProximityMode(bool mode);
+    
+    int readTouchData();
+               
+    unsigned char read(int key);
+    
+    int write(int address, unsigned char value);
+    int writeMany(int start, unsigned char* dataSet, int length);
+
+    void setElectrodeThreshold(int electrodeId, unsigned char touchThreshold, unsigned char releaseThreshold);
+        
+protected:
+    // Configures the MPR with standard settings. This is permitted to be overwritten by sub-classes.
+    void configureSettings();
+    
+private:
+    // The I2C bus instance.
+    I2C *i2c;
+
+    // i2c address of this mpr121
+    Address address;
+};
+
+
+// MPR121 Register Defines
+#define    MHD_R        0x2B
+#define    NHD_R        0x2C
+#define    NCL_R        0x2D
+#define    FDL_R        0x2E
+#define    MHD_F        0x2F
+#define    NHD_F        0x30
+#define    NCL_F        0x31
+#define    FDL_F        0x32
+#define    NHDT         0x33
+#define    NCLT         0x34
+#define    FDLT         0x35
+// Proximity sensing controls
+#define    MHDPROXR     0x36
+#define    NHDPROXR     0x37
+#define    NCLPROXR     0x38
+#define    FDLPROXR     0x39
+#define    MHDPROXF     0x3A
+#define    NHDPROXF     0x3B
+#define    NCLPROXF     0x3C
+#define    FDLPROXF     0x3D
+#define    NHDPROXT     0x3E
+#define    NCLPROXT     0x3F
+#define    FDLPROXT     0x40
+// Electrode Touch/Release thresholds
+#define    ELE0_T       0x41
+#define    ELE0_R       0x42
+#define    ELE1_T       0x43
+#define    ELE1_R       0x44
+#define    ELE2_T       0x45
+#define    ELE2_R       0x46
+#define    ELE3_T       0x47
+#define    ELE3_R       0x48
+#define    ELE4_T       0x49
+#define    ELE4_R       0x4A
+#define    ELE5_T       0x4B
+#define    ELE5_R       0x4C
+#define    ELE6_T       0x4D
+#define    ELE6_R       0x4E
+#define    ELE7_T       0x4F
+#define    ELE7_R       0x50
+#define    ELE8_T       0x51
+#define    ELE8_R       0x52
+#define    ELE9_T       0x53
+#define    ELE9_R       0x54
+#define    ELE10_T      0x55
+#define    ELE10_R      0x56
+#define    ELE11_T      0x57
+#define    ELE11_R      0x58
+// Proximity Touch/Release thresholds
+#define    EPROXTTH     0x59
+#define    EPROXRTH     0x5A
+// Debounce configuration
+#define    DEB_CFG      0x5B
+// AFE- Analogue Front End configuration
+#define    AFE_CFG      0x5C 
+// Filter configuration
+#define    FIL_CFG      0x5D
+// Electrode configuration - transistions to "active mode"
+#define    ELE_CFG      0x5E
+
+#define GPIO_CTRL0      0x73
+#define GPIO_CTRL1      0x74
+#define GPIO_DATA       0x75
+#define    GPIO_DIR     0x76
+#define    GPIO_EN      0x77
+#define    GPIO_SET     0x78
+#define GPIO_CLEAR      0x79
+#define GPIO_TOGGLE     0x7A
+// Auto configration registers
+#define    AUTO_CFG_0   0x7B
+#define    AUTO_CFG_U   0x7D
+#define    AUTO_CFG_L   0x7E
+#define    AUTO_CFG_T   0x7F
+
+// Threshold defaults
+// Electrode touch threshold
+#define    E_THR_T      0x0F
+// Electrode release threshold 
+#define    E_THR_R      0x0A
+// Prox touch threshold
+#define    PROX_THR_T   0x02
+// Prox release threshold
+#define    PROX_THR_R   0x02
+
+#endif
diff -r 000000000000 -r 8f759cbfc3c9 statechart.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/statechart.cpp	Mon Apr 06 20:29:33 2015 +0000
@@ -0,0 +1,705 @@
+#include <mbed.h>
+
+#include "WS2811.h"
+#include "Colors.h"
+
+#include <cmath>
+#include <statechart.h>
+
+/*  TODO LIST
+        [!] FSM for behavior
+        [!] Simplify FSM for practicality
+        [!] Write State Transitions
+        [!] Validate Transitions
+        [!] Validate Speeds, Accels
+        [!] Write State Actions
+        [!] Test LOOK_LEFT action
+        [!] Test LOOK_RIGHT action
+        [!] Test FLOP action
+        [ ] Test WALK action
+        [!] Test FLAP action
+*/
+
+void Statechart::fishStatechart(int netTime, int capNum, int prox, float* xyz, Serial& maestro, WS2811& lightStrip1, WS2811& lightStrip2, float battery) {
+    static actState_t actState = REST;
+    static emoState_t emoState = HAPPY;
+    static int timeAtManeuverStart = 0;
+    
+    /*
+    // IMPORTANT: NEVER RUN THE LIPO BATTERIES BELOW 80% OF THEIR MAX CAPACITY
+    if( actState != BATTERY_LOW && battery < 0.57 ) {
+        actState = BATTERY_LOW;
+        timeAtManeuverStart = netTime;
+    }*/
+    
+    bool cap[12] =             {false,false,false,
+                                false,false,false,
+                                false,false,false,
+                                false,false,false};
+    int i;
+    int temp = capNum;
+    bool touched = false;
+    for (i=0; i<12; i++) {
+        if (((temp>>i)&0x01)==1) {
+            cap[i] = true;
+            touched = true;
+        }
+    }
+    
+    bool trans = (( rand() % 100 ) == 0);
+    int randNum;
+    uint16_t p0, p1, p2;
+    
+    // state transitions
+    switch(actState) {
+        case REST:
+            switch(emoState) {
+                case CURIOUS:
+                    //if touched REST & HAPPY
+                    if( touched ) {
+                        emoState = HAPPY;
+                    }
+                    //LOOK_LEFT, LOOK_RIGHT
+                    else if( trans ) {
+                        randNum = rand() % 2;
+                        if( randNum == 0 ) {
+                            actState = LOOK_LEFT;
+                            timeAtManeuverStart = netTime;
+                        } else if( randNum == 1  ) {
+                            actState = LOOK_RIGHT;
+                            timeAtManeuverStart = netTime;
+                        }
+                    }
+                    break;
+                case MAD:
+                    //if touched back LOOK_LEFT
+                    if( cap[6] || cap[9] ) {
+                        actState = LOOK_LEFT;
+                        timeAtManeuverStart = netTime;
+                    }
+                    //if touched front LOOK_RIGHT
+                    else if( cap[7] || cap[10] ) {
+                        actState = LOOK_RIGHT;
+                        timeAtManeuverStart = netTime;
+                    }
+                    //FLAP
+                    else if( cap[0] || cap[1] || cap[2] || cap[3] || cap[4] || cap[5] ) {
+                        actState = FLAP;
+                        timeAtManeuverStart = netTime;
+                    }
+                    break;
+                case HAPPY:
+                    //if touched FLAP
+                    if( cap[6] || cap[7] || cap[9] || cap[10] ) {
+                        actState = FLAP;
+                        timeAtManeuverStart = netTime;
+                    }
+                    //REST & MAD
+                    else if( cap[0] || cap[1] || cap[2] || cap[3] || cap[4] || cap[5] ) {
+                        emoState = MAD;
+                        timeAtManeuverStart = netTime;
+                    } 
+                    break;
+                case SAD:
+                    //FLOP
+                    if( cap[6] || cap[7] || cap[9] || cap[10] ) {
+                        actState = FLOP;
+                    } else if( cap[0] || cap[1] || cap[2] || cap[3] || cap[4] || cap[5] ) {
+                        actState = FLAP;
+                        timeAtManeuverStart = netTime;
+                    }
+                    break;
+            }   //switch(emoState) {
+            break;  //case REST:
+        case LOOK_LEFT:
+            switch(emoState) {
+                case CURIOUS:
+                    //REST
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        actState = REST;
+                        timeAtManeuverStart = netTime;
+                    }
+                    break;
+                case MAD:  //HEY
+                    //REST
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        actState = REST;
+                        timeAtManeuverStart = netTime;
+                    }
+                    break;
+                case HAPPY:
+                    //
+                    break;
+                case SAD:
+                    //
+                    break;
+            }   //switch(emoState) {
+            break;  //case LOOK_LEFT:
+        case LOOK_RIGHT:
+            switch(emoState) {
+                case CURIOUS:
+                    //REST
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        actState = REST;
+                        timeAtManeuverStart = netTime;
+                    }
+                    break;
+                case MAD:  //HEY
+                    //REST
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        actState = REST;
+                        timeAtManeuverStart = netTime;
+                    }
+                    break;
+                case HAPPY:
+                    //
+                    break;
+                case SAD:
+                    //
+                    break;
+            }   //switch(emoState) {
+            break;  //case LOOK_RIGHT:
+        case FLOP:
+            switch(emoState) {
+                case CURIOUS:
+                    //
+                    break;
+                case MAD:
+                    //
+                    break;
+                case HAPPY:
+                    //
+                    break;
+                case SAD:
+                    //REST
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        actState = REST;
+                        timeAtManeuverStart = netTime;
+                    }
+                    break;
+            }   //switch(emoState) {
+            break;  //case FLOP:
+        case WALK:
+            switch(emoState) {
+                case CURIOUS:
+                    //
+                    break;
+                case MAD:
+                    //
+                    break;
+                case HAPPY:
+                    //
+                    break;
+                case SAD:
+                    //
+                    break;
+            }   //switch(emoState) {
+            break;  //case WALK:
+        case FLAP:
+            switch(emoState) {
+                case CURIOUS:
+                    //
+                    break;
+                case MAD:  //Tantrum
+                    //if touched REST & HAPPY
+                    if( touched ) {
+                        actState = REST;
+                        emoState = SAD;
+                    }
+                    break;
+                case HAPPY:
+                    //REST
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        actState = REST;
+                        timeAtManeuverStart = netTime;
+                    }
+                    break;
+                case SAD:  //Sleep
+                    //if touched REST & HAPPY
+                    if( touched ) {
+                        actState = REST;
+                        emoState = CURIOUS;
+                        timeAtManeuverStart = netTime;
+                    }
+                    break;
+            }   //switch(emoState) {
+            break;  //case FLAP:
+        case BATTERY_LOW:
+            break;
+    }   //switch(actState) {
+    
+    // define what states do
+    switch(actState){
+        case REST:
+            switch( emoState ) {
+                case CURIOUS:
+                    showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+                    showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+                    setServoValue(maestro, 1, 50, 50, 50);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    p0 = 6200;
+                    p1 = 6200;
+                    p2 = 6200;
+                    break;
+                case MAD:
+                    if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) {
+                        showSolidColorBright(lightStrip1, 255, 0, 0, 0.5);
+                        showSolidColorBright(lightStrip2, 255, 0, 0, 0.5);
+                    } else {
+                        showSolidColorBright(lightStrip1, 255, 0, 0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
+                        showSolidColorBright(lightStrip2, 255, 0, 0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
+                    }
+                    setServoValue(maestro, 1, 60, 60, 60);
+                    setServoValue(maestro, 2, 30, 30, 30);
+                    p0 = 6200;
+                    p1 = 6200;
+                    p2 = 6200;
+                    break;
+                case HAPPY:
+                    showRainbow(lightStrip1, 1.0, MINBRITE + (MAXBRITE - MINBRITE) * abs(xyz[0]), 0.5 + 0.5 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000));
+                    showRainbow(lightStrip2, 1.0, MINBRITE + (MAXBRITE - MINBRITE) * abs(xyz[1]), 0.5 + 0.5 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000));
+                    setServoValue(maestro, 1, 100, 100, 100);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    p0 = 6200;
+                    p1 = 6200;
+                    p2 = 6200;
+                    break;
+                case SAD:
+                    if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) {
+                        showSolidColorBright(lightStrip1, 0, 0, 255, 0.2);
+                        showSolidColorBright(lightStrip2, 0, 0, 255, 0.2);
+                    } else {
+                        showSolidColorBright(lightStrip1, 0, 0, 255, (float)(0.35 - 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
+                        showSolidColorBright(lightStrip2, 0, 0, 255, (float)(0.35 - 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
+                    }
+                    setServoValue(maestro, 1, 20, 20, 20);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    p0 = 6200;
+                    p1 = 6200;
+                    p2 = 6200;
+                    break;
+            }   //switch(emoState) {
+            WS2811::startDMA();
+            setServoValue(maestro, 0, p0, p1, p2);
+            break;  //case REST:
+        case LOOK_LEFT:
+            switch( emoState ) {
+                case CURIOUS:  //Search >.>
+                    showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.4 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
+                    showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.3 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
+                    setServoValue(maestro, 1, 50, 50, 50);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        p0 = 6000;
+                        p1 = 6000;
+                        p2 = 6000;
+                    } else {
+                        p0 = 3000;
+                        p1 = 8000;
+                        p2 = 6200;
+                    }
+                    break;
+                case MAD:  //HEY  >:(
+                    showSolidColorBright(lightStrip1, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
+                    showSolidColorBright(lightStrip2, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
+                    setServoValue(maestro, 1, 100, 100, 100);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        p0 = 6000;
+                        p1 = 6000;
+                        p2 = 6000;
+                    } else {
+                        p0 = 3000;
+                        p1 = 8000;
+                        p2 = 6200;
+                    }
+                    break;
+                case HAPPY:  //Nudge  '.^
+                    showSolidColorBright(lightStrip1, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));
+                    showSolidColorBright(lightStrip2, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));
+                    setServoValue(maestro, 1, 70, 70, 70);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        p0 = 6000;
+                        p1 = 6000;
+                        p2 = 6000;
+                    } else {
+                        p0 = 3000;
+                        p1 = 8000;
+                        p2 = 6200;
+                    }
+                    break;
+                case SAD:  //Unreachable state O.O
+                    showSolidColor(lightStrip1, 0, 0, 0);
+                    showSolidColor(lightStrip2, 0, 0, 0);
+                    setServoValue(maestro, 1, 10, 10, 10);
+                    setServoValue(maestro, 2, 10, 10, 10);
+                    p0 = 6000;
+                    p1 = 6000;
+                    p2 = 6000;
+                    break;
+            }   //switch(emoState) {
+            WS2811::startDMA();
+            setServoValue(maestro, 0, p0, p1, p2);
+            break;  //case LOOK_LEFT:
+        case LOOK_RIGHT:
+            switch( emoState ) {
+                case CURIOUS:  //Search <.<
+                    showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.4 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
+                    showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.3 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
+                    setServoValue(maestro, 1, 50, 50, 50);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        p0 = 6000;
+                        p1 = 6000;
+                        p2 = 6000;
+                    } else {
+                        p0 = 5000;
+                        p1 = 10000;
+                        p2 = 6200;
+                    }
+                    break;
+                case MAD:  //HEY  >:(
+                    showSolidColorBright(lightStrip1, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
+                    showSolidColorBright(lightStrip2, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
+                    setServoValue(maestro, 1, 120, 120, 120);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        p0 = 6000;
+                        p1 = 6000;
+                        p2 = 6000;
+                    } else {
+                        p0 = 5000;
+                        p1 = 10000;
+                        p2 = 6200;
+                    }
+                    break;
+                case HAPPY:  //Nudge  '.^
+                    showSolidColorBright(lightStrip1, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));
+                    showSolidColorBright(lightStrip2, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));                   
+                    setServoValue(maestro, 1, 70, 70, 70);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    if( netTime - timeAtManeuverStart > 3000 ) {
+                        p0 = 6000;
+                        p1 = 6000;
+                        p2 = 6000;
+                    } else {
+                        p0 = 5000;
+                        p1 = 10000;
+                        p2 = 6200;
+                    }
+                    break;
+                case SAD:  //Unreachable state O.O
+                    showSolidColor(lightStrip1, 255, 255, 255);
+                    showSolidColor(lightStrip2, 255, 255, 255);
+                    setServoValue(maestro, 1, 10, 10, 10);
+                    setServoValue(maestro, 2, 10, 10, 10);
+                    p0 = 6000;
+                    p1 = 6000;
+                    p2 = 6000;
+                    break;
+            }   //switch(emoState) {
+            WS2811::startDMA();
+            setServoValue(maestro, 0, p0, p1, p2);
+            break;  //case LOOK_RIGHT
+        case FLOP:
+            switch( emoState ) {
+                case CURIOUS:  //Unreachable state O.O
+                    showSolidColor(lightStrip1, 0, 0, 0);
+                    showSolidColor(lightStrip2, 0, 0, 0);
+                    setServoValue(maestro, 1, 50, 50, 50);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    p0 = 6000;
+                    p1 = 6000;
+                    p2 = 6000;
+                    break;
+                case MAD:  //Unreachable state O.O
+                    showSolidColor(lightStrip1, 0, 0, 0);
+                    showSolidColor(lightStrip2, 0, 0, 0);
+                    setServoValue(maestro, 1, 50, 50, 50);
+                    setServoValue(maestro, 2, 50, 50, 50);
+                    p0 = 6000;
+                    p1 = 6000;
+                    p2 = 6000;
+                    break;
+                case HAPPY:  //Hey!  :D
+                    if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) {
+                        showRainbow(lightStrip1, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/500)), abs(xyz[0]));
+                        showRainbow(lightStrip2, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/500)), abs(xyz[1]));
+                    } else {
+                        showRainbow(lightStrip1, 1.0, 0.2, abs(xyz[0]));
+                        showRainbow(lightStrip2, 1.0, 0.2, abs(xyz[1]));
+                    }
+                    setServoValue(maestro, 1, 80, 80, 80);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    p0 = 4000;
+                    if( (netTime - timeAtManeuverStart) % 4000 > 2000 ) {
+                        p1 = 6200;
+                        p2 = 6200;
+                    } else {
+                        p1 = 2500;
+                        p2 = 2500;
+                    }
+                    break;
+                case SAD:  //Sigh...  :/
+                    showSolidColorBright(lightStrip1, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+                    showSolidColorBright(lightStrip2, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+                    setServoValue(maestro, 1, 20, 20, 20);
+                    setServoValue(maestro, 2, 10, 10, 10);
+                    p0 = 4000;
+                    if( (netTime - timeAtManeuverStart) % 4000 > 1000 ) {
+                        p1 = 6200;
+                        p2 = 6200;
+                    } else {
+                        p1 = 2500;
+                        p2 = 2500;
+                    }
+                    break;
+            }   //switch(emoState) {
+            WS2811::startDMA();
+            setServoValue(maestro, 0, p0, p1, p2);
+            break;  //case FLOP:
+        case WALK:
+            switch( emoState ) {
+                case CURIOUS:  //Search o.o
+                    if( (netTime - timeAtManeuverStart) % 3000 > 2000 ) {
+                        showSomeRainbow(lightStrip1, 0.45 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 0.82 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+                        showSomeRainbow(lightStrip2, 0.45 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 0.82 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+                    } else {
+                        showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+                        showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+                    }
+                    setServoValue(maestro, 1, 70, 70, 70);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    if( (netTime - timeAtManeuverStart) % 1000 < 100 ) {
+                        p0 = 4500;
+                        p1 = 2000;
+                        p2 = 3500;
+                    } else if( (netTime - timeAtManeuverStart) % 1000 < 200 ) {
+                        p0 = 6200;
+                        p1 = 3500;
+                        p2 = 2500;
+                    } else if( (netTime - timeAtManeuverStart) % 1000 < 300 ) {
+                        p0 = 10000;
+                        p1 = 5500;
+                        p2 = 3500;
+                    } else if( (netTime - timeAtManeuverStart) % 1000 < 400 ) {
+                        p0 = 10000;
+                        p1 = 6500;
+                        p2 = 4000;
+                    } else if( (netTime - timeAtManeuverStart) % 1000 < 550 ) {
+                        p0 = 10000;
+                        p1 = 7000;
+                        p2 = 5000;
+                    } else if( (netTime - timeAtManeuverStart) % 1000 < 700 ) {
+                        p0 = 6200;
+                        p1 = 6200;
+                        p2 = 6200;
+                    } else if( (netTime - timeAtManeuverStart) % 1000 < 850 ) {
+                        p0 = 5500;
+                        p1 = 4500;
+                        p2 = 4500;
+                    } else if( (netTime - timeAtManeuverStart) % 1000 < 1000 ) {
+                        p0 = 5000;
+                        p1 = 4000;
+                        p2 = 4000;
+                    }
+                    break;
+                case MAD:  //Unreachable state O.O
+                    showSolidColor(lightStrip1, 0, 0, 0);
+                    showSolidColor(lightStrip2, 0, 0, 0);
+                    setServoValue(maestro, 1, 50, 50, 50);
+                    setServoValue(maestro, 2, 50, 50, 50);
+                    p0 = 6000;
+                    p1 = 6000;
+                    p2 = 6000;
+                    setServoValue(maestro, 0, 6000, 6000, 6000);
+                    break;
+                case HAPPY:  //Chase XD
+                    showRainbow(lightStrip1, 1.0, 0.5, (float)((netTime - timeAtManeuverStart) % 500) / 500.0);
+                    showRainbow(lightStrip2, 1.0, 0.5, (float)((netTime - timeAtManeuverStart) % 500) / 500.0);                   
+                    setServoValue(maestro, 1, 100, 100, 100);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    p0 = (int)(7000 + 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 0*2*PI/3)));
+                    p1 = (int)(5000 - 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 1*2*PI/3)));
+                    p2 = (int)(7000 + 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 2*2*PI/3)));
+                    break;
+                case SAD:  //Unreachable state O.O
+                    showSolidColor(lightStrip1, 0, 0, 0);
+                    showSolidColor(lightStrip2, 0, 0, 0);
+                    setServoValue(maestro, 1, 10, 10, 10);
+                    setServoValue(maestro, 2, 10, 10, 10);
+                    p0 = 6000;
+                    p1 = 6000;
+                    p2 = 6000;
+                    break;
+            }   //switch(emoState) {
+            WS2811::startDMA();
+            setServoValue(maestro, 0, p0, p1, p2);
+            break;  //case WALK:
+        case FLAP:
+            switch( emoState ) {
+                case CURIOUS:  //Unreachable state O.O
+                    showSolidColor(lightStrip1, 0, 0, 0);
+                    showSolidColor(lightStrip2, 0, 0, 0);
+                    setServoValue(maestro, 1, 50, 50, 50);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    p0 = 6000;
+                    p1 = 6000;
+                    p2 = 6000;
+                    break;
+                case MAD:  //Tantrum >:X
+                    showSolidColorBright(lightStrip1, 153, 0, 0, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750)));
+                    showSolidColorBright(lightStrip2, 153, 0, 0, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750)));
+                    setServoValue(maestro, 1, 100, 100, 100);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    p0 = 4000;
+                    p1 = 6200 - 7400 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750);
+                    p2 = 6200 - 7400 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750);
+                    break;
+                case HAPPY:  //Laugh  ^.^
+                    showRainbow(lightStrip1, 1.0, 0.35, (float)((netTime - timeAtManeuverStart) % 1000) / 1000.0);
+                    showRainbow(lightStrip2, 1.0, 0.35, (float)((netTime - timeAtManeuverStart) % 1000) / 1000.0);                 
+                    setServoValue(maestro, 1, 100, 100, 100);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    p0 = 4000;
+                    p1 = 6200;
+                    p2 = 3600 - 800 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/500);
+                    break;
+                case SAD:  //Sleep -_-
+                    showSolidColorBright(lightStrip1, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+                    showSolidColorBright(lightStrip2, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+                    setServoValue(maestro, 1, 20, 20, 20);
+                    setServoValue(maestro, 2, 0, 0, 0);
+                    p0 = 4000;
+                    p1 = 6500;
+                    p2 = 3200 - 1600 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000);
+                    break;
+            }   //switch(emoState) {
+            WS2811::startDMA();
+            setServoValue(maestro, 0, p0, p1, p2);
+            break;  //case FLAP:
+        case BATTERY_LOW:
+            if( (netTime - timeAtManeuverStart) % 1000 > 500 ) {
+                showSolidColor(lightStrip1, 255, 0, 0);
+                showSolidColor(lightStrip2, 0, 0, 0);
+            } else {
+                showSolidColor(lightStrip1, 0, 0, 0);
+                showSolidColor(lightStrip2, 255, 0, 0);
+            }
+            WS2811::startDMA();
+            break;
+    }   //switch(actState) {
+}   //void Statechart::fishStateChart
+
+// @brief sets different colors in each of the LEDs of a strip
+// @param strip the light strip
+// @param sat saturation, 0.0 - 1.0
+// @param brite brightness, 0.0 - 1.0
+// @param hueShift shift, 0.0 - 1.0 is equivalent to 0 - 360 degrees
+void Statechart::showRainbow(WS2811 &strip, float sat, float brite, float hueShift)
+{
+    unsigned nLEDs = strip.numPixels();
+    for (unsigned i = 0; i < nLEDs; i++) {
+        uint8_t r, g, b;
+        float hue = ((float)i / (float)nLEDs) + hueShift;
+        HSBtoRGB(hue, sat, brite, &r, &g, &b);
+        strip.setPixelColor(i, r, g, b);
+    }
+    strip.show();
+}
+
+void Statechart::showSolidColor(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b)
+{
+    unsigned nLEDs = strip.numPixels();
+    for (unsigned i = 0; i < nLEDs; i++) {
+        strip.setPixelColor(i, r, g, b);
+    }
+    strip.show();
+}
+
+
+//showSolidColorBright(lightStrip1, 0, 0, 255, (float)(0.35 + 0.15 * sin(2*PI * (double)currTime / 1000)));
+//showSolidColorBright(lightStrip2, 0, 0, 255, (float)(0.35 + 0.15 * sin(2*PI * (double)currTime / 1000)));
+//WS2811::startDMA();
+void Statechart::showSolidColorBright(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b, float newBrite)
+{
+    unsigned nLEDs = strip.numPixels();
+    for (unsigned i = 0; i < nLEDs; i++) {
+        float hsbVals[3];
+        RGBtoHSB(r, g, b, hsbVals);
+        HSBtoRGB(hsbVals[0], hsbVals[1], newBrite, &r, &g, &b);
+        strip.setPixelColor(i, r, g, b);
+    }
+    strip.show();
+}
+
+void Statechart::showSomeRainbow(WS2811 &strip, float hueLow, float hueHigh, float sat, float brite)
+{
+    unsigned nLEDs = strip.numPixels();
+    for (unsigned i = 0; i < nLEDs; i++) {
+        uint8_t r, g, b;
+        float hue = hueLow + (hueHigh - hueLow) * ((float)i / (float)nLEDs);
+        HSBtoRGB(hue, sat, brite, &r, &g, &b);
+        strip.setPixelColor(i, r, g, b);
+    }
+    strip.show();
+}
+
+/* 
+    Set Servo Value for:
+        Servo Position:                                     index = 0
+            Position is in quarter usec.
+            Neutral ~ 6200
+        Servo Speed:                                        index = 1
+            Speed is quarter usec / ten msec.
+            Unlimited Speed = 0
+        Servo Accel:                                        index = 2
+            Accel is quarter usec / ten msec / eighty msec.
+            Unlimited Accel = 0
+*/
+void Statechart::setServoValue(Serial &maestro, int index, uint16_t x0, uint16_t x1, uint16_t x2) {
+    uint8_t cmd;
+    if( index == 0 ) {
+        cmd = 0x04;
+    } else if( index == 1 ) {
+        cmd = 0x07;
+    } else if( index == 2 ) {
+        cmd =  0x09;
+    } else {
+        return;
+    }
+    
+    uint8_t top7;
+    uint8_t bottom7;
+    
+    bottom7 = x0 & 0x7F;
+    top7 = (x0 & 0x3F80) >> 7;
+    
+    maestro.putc(0xAA);
+    maestro.putc(0x0C);
+    maestro.putc(cmd);
+    maestro.putc(0x00);
+    maestro.putc(bottom7);
+    maestro.putc(top7);
+    
+    bottom7 = x1 & 0x7F;
+    top7 = (x1 & 0x3F80) >> 7;
+    
+    maestro.putc(0xAA);
+    maestro.putc(0x0C);
+    maestro.putc(cmd);
+    maestro.putc(0x01);
+    maestro.putc(bottom7);
+    maestro.putc(top7);
+    
+    bottom7 = x2 & 0x7F;
+    top7 = (x2 & 0x3F80) >> 7;
+    
+    maestro.putc(0xAA);
+    maestro.putc(0x0C);
+    maestro.putc(cmd);
+    maestro.putc(0x02);
+    maestro.putc(bottom7);
+    maestro.putc(top7);
+}
diff -r 000000000000 -r 8f759cbfc3c9 statechart.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/statechart.h	Mon Apr 06 20:29:33 2015 +0000
@@ -0,0 +1,38 @@
+#ifndef STATECHART_H
+#define STATECHART_H
+
+#define MINBRITE    (0.2)
+#define MAXBRITE    (0.5)
+#define PI          (3.141592653589793)
+
+class Statechart{
+public:
+    enum actState_t {
+        REST,
+        LOOK_LEFT,
+        LOOK_RIGHT,
+        FLOP,
+        WALK,
+        FLAP,
+        BATTERY_LOW
+    };
+    
+    enum emoState_t {
+        CURIOUS,
+        MAD,
+        HAPPY,
+        SAD
+    };
+    
+    void fishStatechart(int netTime, int capNum, int prox, float* xyz, Serial &maestro, WS2811 &lightStrip1, WS2811 &lightStrip2, float battery);
+    
+private:
+    void showRainbow(WS2811 &strip, float sat, float brite, float hueShift);
+    void showSolidColor(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b);
+    void showSolidColorBright(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b, float newBrite);
+    void showSomeRainbow(WS2811 &strip, float hueLow, float hueHigh, float sat, float brite);
+    void setMaestroSubroutine(Serial &maestro,  int subNum, int parameter, bool withParameter);
+    void setServoValue(Serial &maestro, int index, uint16_t x0, uint16_t x1, uint16_t x2);
+};
+
+#endif