Ryan Feng
StateChart defining behavior for FlippityFlappity project
Dependencies: MMA8451Q Multi_WS2811 TSI mbed
Diff: statechart.cpp
- Revision:
- 0:e72b30045834
--- /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);
+}