File content as of revision 0:e72b30045834:

#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 = CURIOUS;
    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;
                    }
                    //if !touched REST & MAD
                    //LOOK_LEFT, LOOK_RIGHT, WALK
                    else if( trans ) {
                        randNum = rand() % 4;
                        if( randNum == 0 ) {
                            actState = LOOK_LEFT;
                            timeAtManeuverStart = netTime;
                        } else if( randNum == 1  ) {
                            actState = LOOK_RIGHT;
                            timeAtManeuverStart = netTime;
                        } else if( randNum == 2 ) {
                            actState = WALK;
                            timeAtManeuverStart = netTime;
                        } else if( randNum == 3 ) {
                            emoState = MAD;
                            timeAtManeuverStart = netTime;
                        }
                    }
                    break;
                case MAD:
                    //if touched back LOOK_LEFT
                    if( cap[0] || cap[3] || cap[6] || cap[9] ) {
                        actState = LOOK_LEFT;
                        timeAtManeuverStart = netTime;
                    }
                    //if touched front LOOK_RIGHT
                    else if( cap[1] || cap[4] || cap[7] || cap[10] ) {
                        actState = LOOK_RIGHT;
                        timeAtManeuverStart = netTime;
                    }
                    //FLAP
                    else if( trans && (netTime - timeAtManeuverStart) > 1500 ) {
                        actState = FLAP;
                        timeAtManeuverStart = netTime;
                    }
                    break;
                case HAPPY:
                    //if touched FLAP
                    if( touched ) {
                        actState = FLAP;
                        timeAtManeuverStart = netTime;
                    }
                    //FLOP or REST & CURIOUS
                    else if( trans ) {
                        randNum = rand() % 3;
                        if( randNum < 2 ) {
                            actState = FLOP;
                            timeAtManeuverStart = netTime;
                        } else if( randNum == 2 ) {
                            actState = REST;
                            emoState = CURIOUS;
                            timeAtManeuverStart = netTime;
                        }
                    } 
                    break;
                case SAD:
                    //FLOP
                    if( trans ) {
                        actState = FLOP;
                        timeAtManeuverStart = netTime;
                    }
                    break;
            }   //switch(emoState) {
            break;  //case REST:
        case LOOK_LEFT:
            switch(emoState) {
                case CURIOUS:
                    //REST, LOOK_RIGHT
                    if( trans || netTime - timeAtManeuverStart > 3000 ) {
                        randNum = rand() % 2;
                        if( randNum == 0 ) {
                            actState = REST;
                            timeAtManeuverStart = netTime;
                        } else if( randNum == 1 ) {
                            actState = LOOK_RIGHT;
                            timeAtManeuverStart = netTime;
                        }
                    }
                    break;
                case MAD:  //HEY
                    //REST
                    if( trans && netTime - timeAtManeuverStart > 3000 ) {
                        actState = REST;
                        timeAtManeuverStart = netTime;
                    }
                    break;
                case HAPPY:  //Nudge
                    //if touched FLOP
                    if( touched ) {
                        actState = FLOP;
                        timeAtManeuverStart = netTime;
                    }
                    //if !touched REST & SAD
                    //FLOP
                    else if( trans && netTime - timeAtManeuverStart > 3000 ) {
                        randNum = rand() % 2;
                        if( randNum == 0 ) {
                            actState = REST;
                            emoState = SAD;
                            timeAtManeuverStart = netTime;
                        }
                        else if( randNum == 1 ) {
                            actState = FLOP;
                            timeAtManeuverStart = netTime;
                        }
                    }
                    break;
                case SAD:
                    //
                    break;
            }   //switch(emoState) {
            break;  //case LOOK_LEFT:
        case LOOK_RIGHT:
            switch(emoState) {
                case CURIOUS:
                    //REST, LOOK_RIGHT
                    if( trans || netTime - timeAtManeuverStart > 3000 ) {
                        randNum = rand() % 2;
                        if( randNum == 0 ) {
                            actState = REST;
                            timeAtManeuverStart = netTime;
                        } else if ( randNum == 1 ) {
                            actState = LOOK_LEFT;
                            timeAtManeuverStart = netTime;
                        }
                    }
                    break;
                case MAD:  //HEY
                    //REST
                    if( trans && netTime - timeAtManeuverStart > 3000 ) {
                        actState = REST;
                        timeAtManeuverStart = netTime;
                    }
                    break;
                case HAPPY:  //Nudge
                    //if touched FLOP
                    if( touched ) {
                        actState = FLOP;
                        timeAtManeuverStart = netTime;
                    }
                    //if !touched REST & SAD or REST & MAD
                    //FLOP
                    else if( trans && netTime - timeAtManeuverStart > 3000 ) {
                        randNum = rand() % 2;
                        if( randNum == 0 ) {
                            randNum = rand() % 2;
                            if( randNum == 0 ) {
                                actState = REST;
                                emoState = SAD;
                                timeAtManeuverStart = netTime;
                            } else if( randNum == 1 ) {
                                actState = REST;
                                emoState = MAD;
                                timeAtManeuverStart = netTime;
                            }
                        }
                        else if( randNum == 1 ) {
                            actState = FLOP;
                            timeAtManeuverStart = netTime;
                        }
                    }
                    break;
                case SAD:
                    //
                    break;
            }   //switch(emoState) {
            break;  //case LOOK_RIGHT:
        case FLOP:
            switch(emoState) {
                case CURIOUS:
                    //
                    break;
                case MAD:
                    //
                    break;
                case HAPPY:  //Hey!
                    //if touched WALK, FLAP
                    if( touched ) {
                        randNum = rand() % 2;
                        if( randNum == 0 ) {
                            actState = WALK;
                            timeAtManeuverStart = netTime;
                        }
                        else if( randNum == 1 ) {
                            actState = FLAP;
                            timeAtManeuverStart = netTime;
                        }
                    }
                    //REST, LOOK_LEFT, LOOK_RIGHT
                    else if( trans && netTime - timeAtManeuverStart > 500 ) {
                        randNum = rand() % 3;
                        if( randNum == 0 ) {
                            actState = REST;
                            timeAtManeuverStart = netTime;
                        } else if( randNum == 1 ) {
                            actState = LOOK_LEFT;
                            timeAtManeuverStart = netTime;
                        } else if( randNum == 2 ) {
                            actState = LOOK_RIGHT;
                            timeAtManeuverStart = netTime;
                        }
                    }
                    break;
                case SAD:  //Sigh...
                    //if touched REST & HAPPY
                    if( touched ) {
                        actState = REST;
                        emoState = HAPPY;
                        timeAtManeuverStart = netTime;
                    }
                    //REST, FLAP
                    else if( trans && netTime - timeAtManeuverStart > 1000 ) {
                        randNum = rand() % 2;
                        if( randNum == 0 ) {
                            actState = REST;
                            timeAtManeuverStart = netTime;
                        } else if( randNum == 1) {
                            actState = FLAP;
                            timeAtManeuverStart = netTime;
                        }
                    }
                    break;
            }   //switch(emoState) {
            break;  //case FLOP:
        case WALK:
            switch(emoState) {
                case CURIOUS:  //Search
                    //if touched REST & HAPPY
                    if( touched ) {
                        actState = REST;
                        emoState = HAPPY;
                        timeAtManeuverStart = netTime;
                    }
                    //if !touched REST & MAD
                    //REST
                    else if( trans && netTime - timeAtManeuverStart > 3000 ) {
                        randNum = rand() % 3;
                        if( randNum == 2 ) {
                            actState = REST;
                            emoState = MAD;
                            timeAtManeuverStart = netTime;
                        } else if( randNum < 2 ) {
                            actState = REST;
                            timeAtManeuverStart = netTime;
                        }
                    }
                    break;
                case MAD:
                    //
                    break;
                case HAPPY:  //Chase
                    //FLOP
                    if( trans && netTime - timeAtManeuverStart > 3000  ) {
                        actState = FLOP;
                        timeAtManeuverStart = netTime;
                    }
                    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 = HAPPY;
                    }
                    break;
                case HAPPY:  //Laugh
                    //if touched FLAP
                    if( touched ) {
                        timeAtManeuverStart = netTime;
                    }
                    //REST
                    else if( trans && netTime - timeAtManeuverStart > 750 ) {
                        actState = REST;
                        timeAtManeuverStart = netTime;
                    }
                    break;
                case SAD:  //Sleep
                    //if touched REST & HAPPY
                    if( touched ) {
                        actState = REST;
                        emoState = HAPPY;
                        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);
                    p0 = (int)(7000 + 3000 * (0.5 * cos(4*PI*(double)(netTime - timeAtManeuverStart)/5000 + 0*2*PI/3)));
                    p1 = (int)(5000 - 3000 * (0.5 * cos(4*PI*(double)(netTime - timeAtManeuverStart)/5000 + 1*2*PI/3)));
                    p2 = (int)(7000 + 3000 * (0.5 * cos(4*PI*(double)(netTime - timeAtManeuverStart)/5000 + 2*2*PI/3)));
                    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 = 6200;
                    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);
}