![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
StateChart defining behavior for FlippityFlappity project
Dependencies: MMA8451Q Multi_WS2811 TSI mbed
Diff: statechart.cpp
- Revision:
- 0:e72b30045834
diff -r 000000000000 -r e72b30045834 statechart.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/statechart.cpp Mon Apr 06 20:27:47 2015 +0000 @@ -0,0 +1,805 @@ +#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); +}