StateChart for behavior of FlippityFlappity in presentation. Behavior here is predictable and deterministic.
Dependencies: MMA8451Q Multi_WS2811_Demo TSI mbed
statechart.cpp
- Committer:
- ryanfeng
- Date:
- 2015-04-06
- Revision:
- 0:8f759cbfc3c9
File content as of revision 0:8f759cbfc3c9:
#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); }