Ryan Feng / Mbed 2 deprecated FishStateChartDemo

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

Dependencies:   MMA8451Q Multi_WS2811_Demo TSI mbed

statechart.cpp@0:8f759cbfc3c9, 2015-04-06 (annotated)

Committer:
ryanfeng
Date:
Mon Apr 06 20:29:33 2015 +0000
Revision:
0:8f759cbfc3c9
Demo Code for presentation

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanfeng 0:8f759cbfc3c9 1 #include <mbed.h>
ryanfeng 0:8f759cbfc3c9 2
ryanfeng 0:8f759cbfc3c9 3 #include "WS2811.h"
ryanfeng 0:8f759cbfc3c9 4 #include "Colors.h"
ryanfeng 0:8f759cbfc3c9 5
ryanfeng 0:8f759cbfc3c9 6 #include <cmath>
ryanfeng 0:8f759cbfc3c9 7 #include <statechart.h>
ryanfeng 0:8f759cbfc3c9 8
ryanfeng 0:8f759cbfc3c9 9 /* TODO LIST
ryanfeng 0:8f759cbfc3c9 10 [!] FSM for behavior
ryanfeng 0:8f759cbfc3c9 11 [!] Simplify FSM for practicality
ryanfeng 0:8f759cbfc3c9 12 [!] Write State Transitions
ryanfeng 0:8f759cbfc3c9 13 [!] Validate Transitions
ryanfeng 0:8f759cbfc3c9 14 [!] Validate Speeds, Accels
ryanfeng 0:8f759cbfc3c9 15 [!] Write State Actions
ryanfeng 0:8f759cbfc3c9 16 [!] Test LOOK_LEFT action
ryanfeng 0:8f759cbfc3c9 17 [!] Test LOOK_RIGHT action
ryanfeng 0:8f759cbfc3c9 18 [!] Test FLOP action
ryanfeng 0:8f759cbfc3c9 19 [ ] Test WALK action
ryanfeng 0:8f759cbfc3c9 20 [!] Test FLAP action
ryanfeng 0:8f759cbfc3c9 21 */
ryanfeng 0:8f759cbfc3c9 22
ryanfeng 0:8f759cbfc3c9 23 void Statechart::fishStatechart(int netTime, int capNum, int prox, float* xyz, Serial& maestro, WS2811& lightStrip1, WS2811& lightStrip2, float battery) {
ryanfeng 0:8f759cbfc3c9 24 static actState_t actState = REST;
ryanfeng 0:8f759cbfc3c9 25 static emoState_t emoState = HAPPY;
ryanfeng 0:8f759cbfc3c9 26 static int timeAtManeuverStart = 0;
ryanfeng 0:8f759cbfc3c9 27
ryanfeng 0:8f759cbfc3c9 28 /*
ryanfeng 0:8f759cbfc3c9 29 // IMPORTANT: NEVER RUN THE LIPO BATTERIES BELOW 80% OF THEIR MAX CAPACITY
ryanfeng 0:8f759cbfc3c9 30 if( actState != BATTERY_LOW && battery < 0.57 ) {
ryanfeng 0:8f759cbfc3c9 31 actState = BATTERY_LOW;
ryanfeng 0:8f759cbfc3c9 32 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 33 }*/
ryanfeng 0:8f759cbfc3c9 34
ryanfeng 0:8f759cbfc3c9 35 bool cap[12] = {false,false,false,
ryanfeng 0:8f759cbfc3c9 36 false,false,false,
ryanfeng 0:8f759cbfc3c9 37 false,false,false,
ryanfeng 0:8f759cbfc3c9 38 false,false,false};
ryanfeng 0:8f759cbfc3c9 39 int i;
ryanfeng 0:8f759cbfc3c9 40 int temp = capNum;
ryanfeng 0:8f759cbfc3c9 41 bool touched = false;
ryanfeng 0:8f759cbfc3c9 42 for (i=0; i<12; i++) {
ryanfeng 0:8f759cbfc3c9 43 if (((temp>>i)&0x01)==1) {
ryanfeng 0:8f759cbfc3c9 44 cap[i] = true;
ryanfeng 0:8f759cbfc3c9 45 touched = true;
ryanfeng 0:8f759cbfc3c9 46 }
ryanfeng 0:8f759cbfc3c9 47 }
ryanfeng 0:8f759cbfc3c9 48
ryanfeng 0:8f759cbfc3c9 49 bool trans = (( rand() % 100 ) == 0);
ryanfeng 0:8f759cbfc3c9 50 int randNum;
ryanfeng 0:8f759cbfc3c9 51 uint16_t p0, p1, p2;
ryanfeng 0:8f759cbfc3c9 52
ryanfeng 0:8f759cbfc3c9 53 // state transitions
ryanfeng 0:8f759cbfc3c9 54 switch(actState) {
ryanfeng 0:8f759cbfc3c9 55 case REST:
ryanfeng 0:8f759cbfc3c9 56 switch(emoState) {
ryanfeng 0:8f759cbfc3c9 57 case CURIOUS:
ryanfeng 0:8f759cbfc3c9 58 //if touched REST & HAPPY
ryanfeng 0:8f759cbfc3c9 59 if( touched ) {
ryanfeng 0:8f759cbfc3c9 60 emoState = HAPPY;
ryanfeng 0:8f759cbfc3c9 61 }
ryanfeng 0:8f759cbfc3c9 62 //LOOK_LEFT, LOOK_RIGHT
ryanfeng 0:8f759cbfc3c9 63 else if( trans ) {
ryanfeng 0:8f759cbfc3c9 64 randNum = rand() % 2;
ryanfeng 0:8f759cbfc3c9 65 if( randNum == 0 ) {
ryanfeng 0:8f759cbfc3c9 66 actState = LOOK_LEFT;
ryanfeng 0:8f759cbfc3c9 67 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 68 } else if( randNum == 1 ) {
ryanfeng 0:8f759cbfc3c9 69 actState = LOOK_RIGHT;
ryanfeng 0:8f759cbfc3c9 70 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 71 }
ryanfeng 0:8f759cbfc3c9 72 }
ryanfeng 0:8f759cbfc3c9 73 break;
ryanfeng 0:8f759cbfc3c9 74 case MAD:
ryanfeng 0:8f759cbfc3c9 75 //if touched back LOOK_LEFT
ryanfeng 0:8f759cbfc3c9 76 if( cap[6] || cap[9] ) {
ryanfeng 0:8f759cbfc3c9 77 actState = LOOK_LEFT;
ryanfeng 0:8f759cbfc3c9 78 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 79 }
ryanfeng 0:8f759cbfc3c9 80 //if touched front LOOK_RIGHT
ryanfeng 0:8f759cbfc3c9 81 else if( cap[7] || cap[10] ) {
ryanfeng 0:8f759cbfc3c9 82 actState = LOOK_RIGHT;
ryanfeng 0:8f759cbfc3c9 83 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 84 }
ryanfeng 0:8f759cbfc3c9 85 //FLAP
ryanfeng 0:8f759cbfc3c9 86 else if( cap[0] || cap[1] || cap[2] || cap[3] || cap[4] || cap[5] ) {
ryanfeng 0:8f759cbfc3c9 87 actState = FLAP;
ryanfeng 0:8f759cbfc3c9 88 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 89 }
ryanfeng 0:8f759cbfc3c9 90 break;
ryanfeng 0:8f759cbfc3c9 91 case HAPPY:
ryanfeng 0:8f759cbfc3c9 92 //if touched FLAP
ryanfeng 0:8f759cbfc3c9 93 if( cap[6] || cap[7] || cap[9] || cap[10] ) {
ryanfeng 0:8f759cbfc3c9 94 actState = FLAP;
ryanfeng 0:8f759cbfc3c9 95 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 96 }
ryanfeng 0:8f759cbfc3c9 97 //REST & MAD
ryanfeng 0:8f759cbfc3c9 98 else if( cap[0] || cap[1] || cap[2] || cap[3] || cap[4] || cap[5] ) {
ryanfeng 0:8f759cbfc3c9 99 emoState = MAD;
ryanfeng 0:8f759cbfc3c9 100 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 101 }
ryanfeng 0:8f759cbfc3c9 102 break;
ryanfeng 0:8f759cbfc3c9 103 case SAD:
ryanfeng 0:8f759cbfc3c9 104 //FLOP
ryanfeng 0:8f759cbfc3c9 105 if( cap[6] || cap[7] || cap[9] || cap[10] ) {
ryanfeng 0:8f759cbfc3c9 106 actState = FLOP;
ryanfeng 0:8f759cbfc3c9 107 } else if( cap[0] || cap[1] || cap[2] || cap[3] || cap[4] || cap[5] ) {
ryanfeng 0:8f759cbfc3c9 108 actState = FLAP;
ryanfeng 0:8f759cbfc3c9 109 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 110 }
ryanfeng 0:8f759cbfc3c9 111 break;
ryanfeng 0:8f759cbfc3c9 112 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 113 break; //case REST:
ryanfeng 0:8f759cbfc3c9 114 case LOOK_LEFT:
ryanfeng 0:8f759cbfc3c9 115 switch(emoState) {
ryanfeng 0:8f759cbfc3c9 116 case CURIOUS:
ryanfeng 0:8f759cbfc3c9 117 //REST
ryanfeng 0:8f759cbfc3c9 118 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 119 actState = REST;
ryanfeng 0:8f759cbfc3c9 120 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 121 }
ryanfeng 0:8f759cbfc3c9 122 break;
ryanfeng 0:8f759cbfc3c9 123 case MAD: //HEY
ryanfeng 0:8f759cbfc3c9 124 //REST
ryanfeng 0:8f759cbfc3c9 125 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 126 actState = REST;
ryanfeng 0:8f759cbfc3c9 127 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 128 }
ryanfeng 0:8f759cbfc3c9 129 break;
ryanfeng 0:8f759cbfc3c9 130 case HAPPY:
ryanfeng 0:8f759cbfc3c9 131 //
ryanfeng 0:8f759cbfc3c9 132 break;
ryanfeng 0:8f759cbfc3c9 133 case SAD:
ryanfeng 0:8f759cbfc3c9 134 //
ryanfeng 0:8f759cbfc3c9 135 break;
ryanfeng 0:8f759cbfc3c9 136 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 137 break; //case LOOK_LEFT:
ryanfeng 0:8f759cbfc3c9 138 case LOOK_RIGHT:
ryanfeng 0:8f759cbfc3c9 139 switch(emoState) {
ryanfeng 0:8f759cbfc3c9 140 case CURIOUS:
ryanfeng 0:8f759cbfc3c9 141 //REST
ryanfeng 0:8f759cbfc3c9 142 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 143 actState = REST;
ryanfeng 0:8f759cbfc3c9 144 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 145 }
ryanfeng 0:8f759cbfc3c9 146 break;
ryanfeng 0:8f759cbfc3c9 147 case MAD: //HEY
ryanfeng 0:8f759cbfc3c9 148 //REST
ryanfeng 0:8f759cbfc3c9 149 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 150 actState = REST;
ryanfeng 0:8f759cbfc3c9 151 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 152 }
ryanfeng 0:8f759cbfc3c9 153 break;
ryanfeng 0:8f759cbfc3c9 154 case HAPPY:
ryanfeng 0:8f759cbfc3c9 155 //
ryanfeng 0:8f759cbfc3c9 156 break;
ryanfeng 0:8f759cbfc3c9 157 case SAD:
ryanfeng 0:8f759cbfc3c9 158 //
ryanfeng 0:8f759cbfc3c9 159 break;
ryanfeng 0:8f759cbfc3c9 160 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 161 break; //case LOOK_RIGHT:
ryanfeng 0:8f759cbfc3c9 162 case FLOP:
ryanfeng 0:8f759cbfc3c9 163 switch(emoState) {
ryanfeng 0:8f759cbfc3c9 164 case CURIOUS:
ryanfeng 0:8f759cbfc3c9 165 //
ryanfeng 0:8f759cbfc3c9 166 break;
ryanfeng 0:8f759cbfc3c9 167 case MAD:
ryanfeng 0:8f759cbfc3c9 168 //
ryanfeng 0:8f759cbfc3c9 169 break;
ryanfeng 0:8f759cbfc3c9 170 case HAPPY:
ryanfeng 0:8f759cbfc3c9 171 //
ryanfeng 0:8f759cbfc3c9 172 break;
ryanfeng 0:8f759cbfc3c9 173 case SAD:
ryanfeng 0:8f759cbfc3c9 174 //REST
ryanfeng 0:8f759cbfc3c9 175 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 176 actState = REST;
ryanfeng 0:8f759cbfc3c9 177 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 178 }
ryanfeng 0:8f759cbfc3c9 179 break;
ryanfeng 0:8f759cbfc3c9 180 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 181 break; //case FLOP:
ryanfeng 0:8f759cbfc3c9 182 case WALK:
ryanfeng 0:8f759cbfc3c9 183 switch(emoState) {
ryanfeng 0:8f759cbfc3c9 184 case CURIOUS:
ryanfeng 0:8f759cbfc3c9 185 //
ryanfeng 0:8f759cbfc3c9 186 break;
ryanfeng 0:8f759cbfc3c9 187 case MAD:
ryanfeng 0:8f759cbfc3c9 188 //
ryanfeng 0:8f759cbfc3c9 189 break;
ryanfeng 0:8f759cbfc3c9 190 case HAPPY:
ryanfeng 0:8f759cbfc3c9 191 //
ryanfeng 0:8f759cbfc3c9 192 break;
ryanfeng 0:8f759cbfc3c9 193 case SAD:
ryanfeng 0:8f759cbfc3c9 194 //
ryanfeng 0:8f759cbfc3c9 195 break;
ryanfeng 0:8f759cbfc3c9 196 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 197 break; //case WALK:
ryanfeng 0:8f759cbfc3c9 198 case FLAP:
ryanfeng 0:8f759cbfc3c9 199 switch(emoState) {
ryanfeng 0:8f759cbfc3c9 200 case CURIOUS:
ryanfeng 0:8f759cbfc3c9 201 //
ryanfeng 0:8f759cbfc3c9 202 break;
ryanfeng 0:8f759cbfc3c9 203 case MAD: //Tantrum
ryanfeng 0:8f759cbfc3c9 204 //if touched REST & HAPPY
ryanfeng 0:8f759cbfc3c9 205 if( touched ) {
ryanfeng 0:8f759cbfc3c9 206 actState = REST;
ryanfeng 0:8f759cbfc3c9 207 emoState = SAD;
ryanfeng 0:8f759cbfc3c9 208 }
ryanfeng 0:8f759cbfc3c9 209 break;
ryanfeng 0:8f759cbfc3c9 210 case HAPPY:
ryanfeng 0:8f759cbfc3c9 211 //REST
ryanfeng 0:8f759cbfc3c9 212 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 213 actState = REST;
ryanfeng 0:8f759cbfc3c9 214 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 215 }
ryanfeng 0:8f759cbfc3c9 216 break;
ryanfeng 0:8f759cbfc3c9 217 case SAD: //Sleep
ryanfeng 0:8f759cbfc3c9 218 //if touched REST & HAPPY
ryanfeng 0:8f759cbfc3c9 219 if( touched ) {
ryanfeng 0:8f759cbfc3c9 220 actState = REST;
ryanfeng 0:8f759cbfc3c9 221 emoState = CURIOUS;
ryanfeng 0:8f759cbfc3c9 222 timeAtManeuverStart = netTime;
ryanfeng 0:8f759cbfc3c9 223 }
ryanfeng 0:8f759cbfc3c9 224 break;
ryanfeng 0:8f759cbfc3c9 225 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 226 break; //case FLAP:
ryanfeng 0:8f759cbfc3c9 227 case BATTERY_LOW:
ryanfeng 0:8f759cbfc3c9 228 break;
ryanfeng 0:8f759cbfc3c9 229 } //switch(actState) {
ryanfeng 0:8f759cbfc3c9 230
ryanfeng 0:8f759cbfc3c9 231 // define what states do
ryanfeng 0:8f759cbfc3c9 232 switch(actState){
ryanfeng 0:8f759cbfc3c9 233 case REST:
ryanfeng 0:8f759cbfc3c9 234 switch( emoState ) {
ryanfeng 0:8f759cbfc3c9 235 case CURIOUS:
ryanfeng 0:8f759cbfc3c9 236 showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
ryanfeng 0:8f759cbfc3c9 237 showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
ryanfeng 0:8f759cbfc3c9 238 setServoValue(maestro, 1, 50, 50, 50);
ryanfeng 0:8f759cbfc3c9 239 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 240 p0 = 6200;
ryanfeng 0:8f759cbfc3c9 241 p1 = 6200;
ryanfeng 0:8f759cbfc3c9 242 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 243 break;
ryanfeng 0:8f759cbfc3c9 244 case MAD:
ryanfeng 0:8f759cbfc3c9 245 if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) {
ryanfeng 0:8f759cbfc3c9 246 showSolidColorBright(lightStrip1, 255, 0, 0, 0.5);
ryanfeng 0:8f759cbfc3c9 247 showSolidColorBright(lightStrip2, 255, 0, 0, 0.5);
ryanfeng 0:8f759cbfc3c9 248 } else {
ryanfeng 0:8f759cbfc3c9 249 showSolidColorBright(lightStrip1, 255, 0, 0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
ryanfeng 0:8f759cbfc3c9 250 showSolidColorBright(lightStrip2, 255, 0, 0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
ryanfeng 0:8f759cbfc3c9 251 }
ryanfeng 0:8f759cbfc3c9 252 setServoValue(maestro, 1, 60, 60, 60);
ryanfeng 0:8f759cbfc3c9 253 setServoValue(maestro, 2, 30, 30, 30);
ryanfeng 0:8f759cbfc3c9 254 p0 = 6200;
ryanfeng 0:8f759cbfc3c9 255 p1 = 6200;
ryanfeng 0:8f759cbfc3c9 256 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 257 break;
ryanfeng 0:8f759cbfc3c9 258 case HAPPY:
ryanfeng 0:8f759cbfc3c9 259 showRainbow(lightStrip1, 1.0, MINBRITE + (MAXBRITE - MINBRITE) * abs(xyz[0]), 0.5 + 0.5 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000));
ryanfeng 0:8f759cbfc3c9 260 showRainbow(lightStrip2, 1.0, MINBRITE + (MAXBRITE - MINBRITE) * abs(xyz[1]), 0.5 + 0.5 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000));
ryanfeng 0:8f759cbfc3c9 261 setServoValue(maestro, 1, 100, 100, 100);
ryanfeng 0:8f759cbfc3c9 262 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 263 p0 = 6200;
ryanfeng 0:8f759cbfc3c9 264 p1 = 6200;
ryanfeng 0:8f759cbfc3c9 265 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 266 break;
ryanfeng 0:8f759cbfc3c9 267 case SAD:
ryanfeng 0:8f759cbfc3c9 268 if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) {
ryanfeng 0:8f759cbfc3c9 269 showSolidColorBright(lightStrip1, 0, 0, 255, 0.2);
ryanfeng 0:8f759cbfc3c9 270 showSolidColorBright(lightStrip2, 0, 0, 255, 0.2);
ryanfeng 0:8f759cbfc3c9 271 } else {
ryanfeng 0:8f759cbfc3c9 272 showSolidColorBright(lightStrip1, 0, 0, 255, (float)(0.35 - 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
ryanfeng 0:8f759cbfc3c9 273 showSolidColorBright(lightStrip2, 0, 0, 255, (float)(0.35 - 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
ryanfeng 0:8f759cbfc3c9 274 }
ryanfeng 0:8f759cbfc3c9 275 setServoValue(maestro, 1, 20, 20, 20);
ryanfeng 0:8f759cbfc3c9 276 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 277 p0 = 6200;
ryanfeng 0:8f759cbfc3c9 278 p1 = 6200;
ryanfeng 0:8f759cbfc3c9 279 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 280 break;
ryanfeng 0:8f759cbfc3c9 281 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 282 WS2811::startDMA();
ryanfeng 0:8f759cbfc3c9 283 setServoValue(maestro, 0, p0, p1, p2);
ryanfeng 0:8f759cbfc3c9 284 break; //case REST:
ryanfeng 0:8f759cbfc3c9 285 case LOOK_LEFT:
ryanfeng 0:8f759cbfc3c9 286 switch( emoState ) {
ryanfeng 0:8f759cbfc3c9 287 case CURIOUS: //Search >.>
ryanfeng 0:8f759cbfc3c9 288 showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.4 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
ryanfeng 0:8f759cbfc3c9 289 showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.3 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
ryanfeng 0:8f759cbfc3c9 290 setServoValue(maestro, 1, 50, 50, 50);
ryanfeng 0:8f759cbfc3c9 291 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 292 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 293 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 294 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 295 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 296 } else {
ryanfeng 0:8f759cbfc3c9 297 p0 = 3000;
ryanfeng 0:8f759cbfc3c9 298 p1 = 8000;
ryanfeng 0:8f759cbfc3c9 299 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 300 }
ryanfeng 0:8f759cbfc3c9 301 break;
ryanfeng 0:8f759cbfc3c9 302 case MAD: //HEY >:(
ryanfeng 0:8f759cbfc3c9 303 showSolidColorBright(lightStrip1, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
ryanfeng 0:8f759cbfc3c9 304 showSolidColorBright(lightStrip2, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
ryanfeng 0:8f759cbfc3c9 305 setServoValue(maestro, 1, 100, 100, 100);
ryanfeng 0:8f759cbfc3c9 306 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 307 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 308 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 309 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 310 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 311 } else {
ryanfeng 0:8f759cbfc3c9 312 p0 = 3000;
ryanfeng 0:8f759cbfc3c9 313 p1 = 8000;
ryanfeng 0:8f759cbfc3c9 314 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 315 }
ryanfeng 0:8f759cbfc3c9 316 break;
ryanfeng 0:8f759cbfc3c9 317 case HAPPY: //Nudge '.^
ryanfeng 0:8f759cbfc3c9 318 showSolidColorBright(lightStrip1, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));
ryanfeng 0:8f759cbfc3c9 319 showSolidColorBright(lightStrip2, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));
ryanfeng 0:8f759cbfc3c9 320 setServoValue(maestro, 1, 70, 70, 70);
ryanfeng 0:8f759cbfc3c9 321 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 322 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 323 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 324 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 325 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 326 } else {
ryanfeng 0:8f759cbfc3c9 327 p0 = 3000;
ryanfeng 0:8f759cbfc3c9 328 p1 = 8000;
ryanfeng 0:8f759cbfc3c9 329 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 330 }
ryanfeng 0:8f759cbfc3c9 331 break;
ryanfeng 0:8f759cbfc3c9 332 case SAD: //Unreachable state O.O
ryanfeng 0:8f759cbfc3c9 333 showSolidColor(lightStrip1, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 334 showSolidColor(lightStrip2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 335 setServoValue(maestro, 1, 10, 10, 10);
ryanfeng 0:8f759cbfc3c9 336 setServoValue(maestro, 2, 10, 10, 10);
ryanfeng 0:8f759cbfc3c9 337 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 338 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 339 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 340 break;
ryanfeng 0:8f759cbfc3c9 341 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 342 WS2811::startDMA();
ryanfeng 0:8f759cbfc3c9 343 setServoValue(maestro, 0, p0, p1, p2);
ryanfeng 0:8f759cbfc3c9 344 break; //case LOOK_LEFT:
ryanfeng 0:8f759cbfc3c9 345 case LOOK_RIGHT:
ryanfeng 0:8f759cbfc3c9 346 switch( emoState ) {
ryanfeng 0:8f759cbfc3c9 347 case CURIOUS: //Search <.<
ryanfeng 0:8f759cbfc3c9 348 showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.4 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
ryanfeng 0:8f759cbfc3c9 349 showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.3 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
ryanfeng 0:8f759cbfc3c9 350 setServoValue(maestro, 1, 50, 50, 50);
ryanfeng 0:8f759cbfc3c9 351 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 352 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 353 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 354 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 355 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 356 } else {
ryanfeng 0:8f759cbfc3c9 357 p0 = 5000;
ryanfeng 0:8f759cbfc3c9 358 p1 = 10000;
ryanfeng 0:8f759cbfc3c9 359 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 360 }
ryanfeng 0:8f759cbfc3c9 361 break;
ryanfeng 0:8f759cbfc3c9 362 case MAD: //HEY >:(
ryanfeng 0:8f759cbfc3c9 363 showSolidColorBright(lightStrip1, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
ryanfeng 0:8f759cbfc3c9 364 showSolidColorBright(lightStrip2, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
ryanfeng 0:8f759cbfc3c9 365 setServoValue(maestro, 1, 120, 120, 120);
ryanfeng 0:8f759cbfc3c9 366 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 367 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 368 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 369 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 370 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 371 } else {
ryanfeng 0:8f759cbfc3c9 372 p0 = 5000;
ryanfeng 0:8f759cbfc3c9 373 p1 = 10000;
ryanfeng 0:8f759cbfc3c9 374 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 375 }
ryanfeng 0:8f759cbfc3c9 376 break;
ryanfeng 0:8f759cbfc3c9 377 case HAPPY: //Nudge '.^
ryanfeng 0:8f759cbfc3c9 378 showSolidColorBright(lightStrip1, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));
ryanfeng 0:8f759cbfc3c9 379 showSolidColorBright(lightStrip2, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));
ryanfeng 0:8f759cbfc3c9 380 setServoValue(maestro, 1, 70, 70, 70);
ryanfeng 0:8f759cbfc3c9 381 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 382 if( netTime - timeAtManeuverStart > 3000 ) {
ryanfeng 0:8f759cbfc3c9 383 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 384 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 385 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 386 } else {
ryanfeng 0:8f759cbfc3c9 387 p0 = 5000;
ryanfeng 0:8f759cbfc3c9 388 p1 = 10000;
ryanfeng 0:8f759cbfc3c9 389 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 390 }
ryanfeng 0:8f759cbfc3c9 391 break;
ryanfeng 0:8f759cbfc3c9 392 case SAD: //Unreachable state O.O
ryanfeng 0:8f759cbfc3c9 393 showSolidColor(lightStrip1, 255, 255, 255);
ryanfeng 0:8f759cbfc3c9 394 showSolidColor(lightStrip2, 255, 255, 255);
ryanfeng 0:8f759cbfc3c9 395 setServoValue(maestro, 1, 10, 10, 10);
ryanfeng 0:8f759cbfc3c9 396 setServoValue(maestro, 2, 10, 10, 10);
ryanfeng 0:8f759cbfc3c9 397 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 398 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 399 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 400 break;
ryanfeng 0:8f759cbfc3c9 401 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 402 WS2811::startDMA();
ryanfeng 0:8f759cbfc3c9 403 setServoValue(maestro, 0, p0, p1, p2);
ryanfeng 0:8f759cbfc3c9 404 break; //case LOOK_RIGHT
ryanfeng 0:8f759cbfc3c9 405 case FLOP:
ryanfeng 0:8f759cbfc3c9 406 switch( emoState ) {
ryanfeng 0:8f759cbfc3c9 407 case CURIOUS: //Unreachable state O.O
ryanfeng 0:8f759cbfc3c9 408 showSolidColor(lightStrip1, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 409 showSolidColor(lightStrip2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 410 setServoValue(maestro, 1, 50, 50, 50);
ryanfeng 0:8f759cbfc3c9 411 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 412 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 413 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 414 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 415 break;
ryanfeng 0:8f759cbfc3c9 416 case MAD: //Unreachable state O.O
ryanfeng 0:8f759cbfc3c9 417 showSolidColor(lightStrip1, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 418 showSolidColor(lightStrip2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 419 setServoValue(maestro, 1, 50, 50, 50);
ryanfeng 0:8f759cbfc3c9 420 setServoValue(maestro, 2, 50, 50, 50);
ryanfeng 0:8f759cbfc3c9 421 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 422 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 423 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 424 break;
ryanfeng 0:8f759cbfc3c9 425 case HAPPY: //Hey! :D
ryanfeng 0:8f759cbfc3c9 426 if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) {
ryanfeng 0:8f759cbfc3c9 427 showRainbow(lightStrip1, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/500)), abs(xyz[0]));
ryanfeng 0:8f759cbfc3c9 428 showRainbow(lightStrip2, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/500)), abs(xyz[1]));
ryanfeng 0:8f759cbfc3c9 429 } else {
ryanfeng 0:8f759cbfc3c9 430 showRainbow(lightStrip1, 1.0, 0.2, abs(xyz[0]));
ryanfeng 0:8f759cbfc3c9 431 showRainbow(lightStrip2, 1.0, 0.2, abs(xyz[1]));
ryanfeng 0:8f759cbfc3c9 432 }
ryanfeng 0:8f759cbfc3c9 433 setServoValue(maestro, 1, 80, 80, 80);
ryanfeng 0:8f759cbfc3c9 434 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 435 p0 = 4000;
ryanfeng 0:8f759cbfc3c9 436 if( (netTime - timeAtManeuverStart) % 4000 > 2000 ) {
ryanfeng 0:8f759cbfc3c9 437 p1 = 6200;
ryanfeng 0:8f759cbfc3c9 438 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 439 } else {
ryanfeng 0:8f759cbfc3c9 440 p1 = 2500;
ryanfeng 0:8f759cbfc3c9 441 p2 = 2500;
ryanfeng 0:8f759cbfc3c9 442 }
ryanfeng 0:8f759cbfc3c9 443 break;
ryanfeng 0:8f759cbfc3c9 444 case SAD: //Sigh... :/
ryanfeng 0:8f759cbfc3c9 445 showSolidColorBright(lightStrip1, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
ryanfeng 0:8f759cbfc3c9 446 showSolidColorBright(lightStrip2, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
ryanfeng 0:8f759cbfc3c9 447 setServoValue(maestro, 1, 20, 20, 20);
ryanfeng 0:8f759cbfc3c9 448 setServoValue(maestro, 2, 10, 10, 10);
ryanfeng 0:8f759cbfc3c9 449 p0 = 4000;
ryanfeng 0:8f759cbfc3c9 450 if( (netTime - timeAtManeuverStart) % 4000 > 1000 ) {
ryanfeng 0:8f759cbfc3c9 451 p1 = 6200;
ryanfeng 0:8f759cbfc3c9 452 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 453 } else {
ryanfeng 0:8f759cbfc3c9 454 p1 = 2500;
ryanfeng 0:8f759cbfc3c9 455 p2 = 2500;
ryanfeng 0:8f759cbfc3c9 456 }
ryanfeng 0:8f759cbfc3c9 457 break;
ryanfeng 0:8f759cbfc3c9 458 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 459 WS2811::startDMA();
ryanfeng 0:8f759cbfc3c9 460 setServoValue(maestro, 0, p0, p1, p2);
ryanfeng 0:8f759cbfc3c9 461 break; //case FLOP:
ryanfeng 0:8f759cbfc3c9 462 case WALK:
ryanfeng 0:8f759cbfc3c9 463 switch( emoState ) {
ryanfeng 0:8f759cbfc3c9 464 case CURIOUS: //Search o.o
ryanfeng 0:8f759cbfc3c9 465 if( (netTime - timeAtManeuverStart) % 3000 > 2000 ) {
ryanfeng 0:8f759cbfc3c9 466 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)));
ryanfeng 0:8f759cbfc3c9 467 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)));
ryanfeng 0:8f759cbfc3c9 468 } else {
ryanfeng 0:8f759cbfc3c9 469 showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
ryanfeng 0:8f759cbfc3c9 470 showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
ryanfeng 0:8f759cbfc3c9 471 }
ryanfeng 0:8f759cbfc3c9 472 setServoValue(maestro, 1, 70, 70, 70);
ryanfeng 0:8f759cbfc3c9 473 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 474 if( (netTime - timeAtManeuverStart) % 1000 < 100 ) {
ryanfeng 0:8f759cbfc3c9 475 p0 = 4500;
ryanfeng 0:8f759cbfc3c9 476 p1 = 2000;
ryanfeng 0:8f759cbfc3c9 477 p2 = 3500;
ryanfeng 0:8f759cbfc3c9 478 } else if( (netTime - timeAtManeuverStart) % 1000 < 200 ) {
ryanfeng 0:8f759cbfc3c9 479 p0 = 6200;
ryanfeng 0:8f759cbfc3c9 480 p1 = 3500;
ryanfeng 0:8f759cbfc3c9 481 p2 = 2500;
ryanfeng 0:8f759cbfc3c9 482 } else if( (netTime - timeAtManeuverStart) % 1000 < 300 ) {
ryanfeng 0:8f759cbfc3c9 483 p0 = 10000;
ryanfeng 0:8f759cbfc3c9 484 p1 = 5500;
ryanfeng 0:8f759cbfc3c9 485 p2 = 3500;
ryanfeng 0:8f759cbfc3c9 486 } else if( (netTime - timeAtManeuverStart) % 1000 < 400 ) {
ryanfeng 0:8f759cbfc3c9 487 p0 = 10000;
ryanfeng 0:8f759cbfc3c9 488 p1 = 6500;
ryanfeng 0:8f759cbfc3c9 489 p2 = 4000;
ryanfeng 0:8f759cbfc3c9 490 } else if( (netTime - timeAtManeuverStart) % 1000 < 550 ) {
ryanfeng 0:8f759cbfc3c9 491 p0 = 10000;
ryanfeng 0:8f759cbfc3c9 492 p1 = 7000;
ryanfeng 0:8f759cbfc3c9 493 p2 = 5000;
ryanfeng 0:8f759cbfc3c9 494 } else if( (netTime - timeAtManeuverStart) % 1000 < 700 ) {
ryanfeng 0:8f759cbfc3c9 495 p0 = 6200;
ryanfeng 0:8f759cbfc3c9 496 p1 = 6200;
ryanfeng 0:8f759cbfc3c9 497 p2 = 6200;
ryanfeng 0:8f759cbfc3c9 498 } else if( (netTime - timeAtManeuverStart) % 1000 < 850 ) {
ryanfeng 0:8f759cbfc3c9 499 p0 = 5500;
ryanfeng 0:8f759cbfc3c9 500 p1 = 4500;
ryanfeng 0:8f759cbfc3c9 501 p2 = 4500;
ryanfeng 0:8f759cbfc3c9 502 } else if( (netTime - timeAtManeuverStart) % 1000 < 1000 ) {
ryanfeng 0:8f759cbfc3c9 503 p0 = 5000;
ryanfeng 0:8f759cbfc3c9 504 p1 = 4000;
ryanfeng 0:8f759cbfc3c9 505 p2 = 4000;
ryanfeng 0:8f759cbfc3c9 506 }
ryanfeng 0:8f759cbfc3c9 507 break;
ryanfeng 0:8f759cbfc3c9 508 case MAD: //Unreachable state O.O
ryanfeng 0:8f759cbfc3c9 509 showSolidColor(lightStrip1, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 510 showSolidColor(lightStrip2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 511 setServoValue(maestro, 1, 50, 50, 50);
ryanfeng 0:8f759cbfc3c9 512 setServoValue(maestro, 2, 50, 50, 50);
ryanfeng 0:8f759cbfc3c9 513 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 514 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 515 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 516 setServoValue(maestro, 0, 6000, 6000, 6000);
ryanfeng 0:8f759cbfc3c9 517 break;
ryanfeng 0:8f759cbfc3c9 518 case HAPPY: //Chase XD
ryanfeng 0:8f759cbfc3c9 519 showRainbow(lightStrip1, 1.0, 0.5, (float)((netTime - timeAtManeuverStart) % 500) / 500.0);
ryanfeng 0:8f759cbfc3c9 520 showRainbow(lightStrip2, 1.0, 0.5, (float)((netTime - timeAtManeuverStart) % 500) / 500.0);
ryanfeng 0:8f759cbfc3c9 521 setServoValue(maestro, 1, 100, 100, 100);
ryanfeng 0:8f759cbfc3c9 522 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 523 p0 = (int)(7000 + 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 0*2*PI/3)));
ryanfeng 0:8f759cbfc3c9 524 p1 = (int)(5000 - 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 1*2*PI/3)));
ryanfeng 0:8f759cbfc3c9 525 p2 = (int)(7000 + 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 2*2*PI/3)));
ryanfeng 0:8f759cbfc3c9 526 break;
ryanfeng 0:8f759cbfc3c9 527 case SAD: //Unreachable state O.O
ryanfeng 0:8f759cbfc3c9 528 showSolidColor(lightStrip1, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 529 showSolidColor(lightStrip2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 530 setServoValue(maestro, 1, 10, 10, 10);
ryanfeng 0:8f759cbfc3c9 531 setServoValue(maestro, 2, 10, 10, 10);
ryanfeng 0:8f759cbfc3c9 532 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 533 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 534 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 535 break;
ryanfeng 0:8f759cbfc3c9 536 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 537 WS2811::startDMA();
ryanfeng 0:8f759cbfc3c9 538 setServoValue(maestro, 0, p0, p1, p2);
ryanfeng 0:8f759cbfc3c9 539 break; //case WALK:
ryanfeng 0:8f759cbfc3c9 540 case FLAP:
ryanfeng 0:8f759cbfc3c9 541 switch( emoState ) {
ryanfeng 0:8f759cbfc3c9 542 case CURIOUS: //Unreachable state O.O
ryanfeng 0:8f759cbfc3c9 543 showSolidColor(lightStrip1, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 544 showSolidColor(lightStrip2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 545 setServoValue(maestro, 1, 50, 50, 50);
ryanfeng 0:8f759cbfc3c9 546 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 547 p0 = 6000;
ryanfeng 0:8f759cbfc3c9 548 p1 = 6000;
ryanfeng 0:8f759cbfc3c9 549 p2 = 6000;
ryanfeng 0:8f759cbfc3c9 550 break;
ryanfeng 0:8f759cbfc3c9 551 case MAD: //Tantrum >:X
ryanfeng 0:8f759cbfc3c9 552 showSolidColorBright(lightStrip1, 153, 0, 0, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750)));
ryanfeng 0:8f759cbfc3c9 553 showSolidColorBright(lightStrip2, 153, 0, 0, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750)));
ryanfeng 0:8f759cbfc3c9 554 setServoValue(maestro, 1, 100, 100, 100);
ryanfeng 0:8f759cbfc3c9 555 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 556 p0 = 4000;
ryanfeng 0:8f759cbfc3c9 557 p1 = 6200 - 7400 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750);
ryanfeng 0:8f759cbfc3c9 558 p2 = 6200 - 7400 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750);
ryanfeng 0:8f759cbfc3c9 559 break;
ryanfeng 0:8f759cbfc3c9 560 case HAPPY: //Laugh ^.^
ryanfeng 0:8f759cbfc3c9 561 showRainbow(lightStrip1, 1.0, 0.35, (float)((netTime - timeAtManeuverStart) % 1000) / 1000.0);
ryanfeng 0:8f759cbfc3c9 562 showRainbow(lightStrip2, 1.0, 0.35, (float)((netTime - timeAtManeuverStart) % 1000) / 1000.0);
ryanfeng 0:8f759cbfc3c9 563 setServoValue(maestro, 1, 100, 100, 100);
ryanfeng 0:8f759cbfc3c9 564 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 565 p0 = 4000;
ryanfeng 0:8f759cbfc3c9 566 p1 = 6200;
ryanfeng 0:8f759cbfc3c9 567 p2 = 3600 - 800 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/500);
ryanfeng 0:8f759cbfc3c9 568 break;
ryanfeng 0:8f759cbfc3c9 569 case SAD: //Sleep -_-
ryanfeng 0:8f759cbfc3c9 570 showSolidColorBright(lightStrip1, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
ryanfeng 0:8f759cbfc3c9 571 showSolidColorBright(lightStrip2, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
ryanfeng 0:8f759cbfc3c9 572 setServoValue(maestro, 1, 20, 20, 20);
ryanfeng 0:8f759cbfc3c9 573 setServoValue(maestro, 2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 574 p0 = 4000;
ryanfeng 0:8f759cbfc3c9 575 p1 = 6500;
ryanfeng 0:8f759cbfc3c9 576 p2 = 3200 - 1600 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000);
ryanfeng 0:8f759cbfc3c9 577 break;
ryanfeng 0:8f759cbfc3c9 578 } //switch(emoState) {
ryanfeng 0:8f759cbfc3c9 579 WS2811::startDMA();
ryanfeng 0:8f759cbfc3c9 580 setServoValue(maestro, 0, p0, p1, p2);
ryanfeng 0:8f759cbfc3c9 581 break; //case FLAP:
ryanfeng 0:8f759cbfc3c9 582 case BATTERY_LOW:
ryanfeng 0:8f759cbfc3c9 583 if( (netTime - timeAtManeuverStart) % 1000 > 500 ) {
ryanfeng 0:8f759cbfc3c9 584 showSolidColor(lightStrip1, 255, 0, 0);
ryanfeng 0:8f759cbfc3c9 585 showSolidColor(lightStrip2, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 586 } else {
ryanfeng 0:8f759cbfc3c9 587 showSolidColor(lightStrip1, 0, 0, 0);
ryanfeng 0:8f759cbfc3c9 588 showSolidColor(lightStrip2, 255, 0, 0);
ryanfeng 0:8f759cbfc3c9 589 }
ryanfeng 0:8f759cbfc3c9 590 WS2811::startDMA();
ryanfeng 0:8f759cbfc3c9 591 break;
ryanfeng 0:8f759cbfc3c9 592 } //switch(actState) {
ryanfeng 0:8f759cbfc3c9 593 } //void Statechart::fishStateChart
ryanfeng 0:8f759cbfc3c9 594
ryanfeng 0:8f759cbfc3c9 595 // @brief sets different colors in each of the LEDs of a strip
ryanfeng 0:8f759cbfc3c9 596 // @param strip the light strip
ryanfeng 0:8f759cbfc3c9 597 // @param sat saturation, 0.0 - 1.0
ryanfeng 0:8f759cbfc3c9 598 // @param brite brightness, 0.0 - 1.0
ryanfeng 0:8f759cbfc3c9 599 // @param hueShift shift, 0.0 - 1.0 is equivalent to 0 - 360 degrees
ryanfeng 0:8f759cbfc3c9 600 void Statechart::showRainbow(WS2811 &strip, float sat, float brite, float hueShift)
ryanfeng 0:8f759cbfc3c9 601 {
ryanfeng 0:8f759cbfc3c9 602 unsigned nLEDs = strip.numPixels();
ryanfeng 0:8f759cbfc3c9 603 for (unsigned i = 0; i < nLEDs; i++) {
ryanfeng 0:8f759cbfc3c9 604 uint8_t r, g, b;
ryanfeng 0:8f759cbfc3c9 605 float hue = ((float)i / (float)nLEDs) + hueShift;
ryanfeng 0:8f759cbfc3c9 606 HSBtoRGB(hue, sat, brite, &r, &g, &b);
ryanfeng 0:8f759cbfc3c9 607 strip.setPixelColor(i, r, g, b);
ryanfeng 0:8f759cbfc3c9 608 }
ryanfeng 0:8f759cbfc3c9 609 strip.show();
ryanfeng 0:8f759cbfc3c9 610 }
ryanfeng 0:8f759cbfc3c9 611
ryanfeng 0:8f759cbfc3c9 612 void Statechart::showSolidColor(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b)
ryanfeng 0:8f759cbfc3c9 613 {
ryanfeng 0:8f759cbfc3c9 614 unsigned nLEDs = strip.numPixels();
ryanfeng 0:8f759cbfc3c9 615 for (unsigned i = 0; i < nLEDs; i++) {
ryanfeng 0:8f759cbfc3c9 616 strip.setPixelColor(i, r, g, b);
ryanfeng 0:8f759cbfc3c9 617 }
ryanfeng 0:8f759cbfc3c9 618 strip.show();
ryanfeng 0:8f759cbfc3c9 619 }
ryanfeng 0:8f759cbfc3c9 620
ryanfeng 0:8f759cbfc3c9 621
ryanfeng 0:8f759cbfc3c9 622 //showSolidColorBright(lightStrip1, 0, 0, 255, (float)(0.35 + 0.15 * sin(2*PI * (double)currTime / 1000)));
ryanfeng 0:8f759cbfc3c9 623 //showSolidColorBright(lightStrip2, 0, 0, 255, (float)(0.35 + 0.15 * sin(2*PI * (double)currTime / 1000)));
ryanfeng 0:8f759cbfc3c9 624 //WS2811::startDMA();
ryanfeng 0:8f759cbfc3c9 625 void Statechart::showSolidColorBright(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b, float newBrite)
ryanfeng 0:8f759cbfc3c9 626 {
ryanfeng 0:8f759cbfc3c9 627 unsigned nLEDs = strip.numPixels();
ryanfeng 0:8f759cbfc3c9 628 for (unsigned i = 0; i < nLEDs; i++) {
ryanfeng 0:8f759cbfc3c9 629 float hsbVals[3];
ryanfeng 0:8f759cbfc3c9 630 RGBtoHSB(r, g, b, hsbVals);
ryanfeng 0:8f759cbfc3c9 631 HSBtoRGB(hsbVals[0], hsbVals[1], newBrite, &r, &g, &b);
ryanfeng 0:8f759cbfc3c9 632 strip.setPixelColor(i, r, g, b);
ryanfeng 0:8f759cbfc3c9 633 }
ryanfeng 0:8f759cbfc3c9 634 strip.show();
ryanfeng 0:8f759cbfc3c9 635 }
ryanfeng 0:8f759cbfc3c9 636
ryanfeng 0:8f759cbfc3c9 637 void Statechart::showSomeRainbow(WS2811 &strip, float hueLow, float hueHigh, float sat, float brite)
ryanfeng 0:8f759cbfc3c9 638 {
ryanfeng 0:8f759cbfc3c9 639 unsigned nLEDs = strip.numPixels();
ryanfeng 0:8f759cbfc3c9 640 for (unsigned i = 0; i < nLEDs; i++) {
ryanfeng 0:8f759cbfc3c9 641 uint8_t r, g, b;
ryanfeng 0:8f759cbfc3c9 642 float hue = hueLow + (hueHigh - hueLow) * ((float)i / (float)nLEDs);
ryanfeng 0:8f759cbfc3c9 643 HSBtoRGB(hue, sat, brite, &r, &g, &b);
ryanfeng 0:8f759cbfc3c9 644 strip.setPixelColor(i, r, g, b);
ryanfeng 0:8f759cbfc3c9 645 }
ryanfeng 0:8f759cbfc3c9 646 strip.show();
ryanfeng 0:8f759cbfc3c9 647 }
ryanfeng 0:8f759cbfc3c9 648
ryanfeng 0:8f759cbfc3c9 649 /*
ryanfeng 0:8f759cbfc3c9 650 Set Servo Value for:
ryanfeng 0:8f759cbfc3c9 651 Servo Position: index = 0
ryanfeng 0:8f759cbfc3c9 652 Position is in quarter usec.
ryanfeng 0:8f759cbfc3c9 653 Neutral ~ 6200
ryanfeng 0:8f759cbfc3c9 654 Servo Speed: index = 1
ryanfeng 0:8f759cbfc3c9 655 Speed is quarter usec / ten msec.
ryanfeng 0:8f759cbfc3c9 656 Unlimited Speed = 0
ryanfeng 0:8f759cbfc3c9 657 Servo Accel: index = 2
ryanfeng 0:8f759cbfc3c9 658 Accel is quarter usec / ten msec / eighty msec.
ryanfeng 0:8f759cbfc3c9 659 Unlimited Accel = 0
ryanfeng 0:8f759cbfc3c9 660 */
ryanfeng 0:8f759cbfc3c9 661 void Statechart::setServoValue(Serial &maestro, int index, uint16_t x0, uint16_t x1, uint16_t x2) {
ryanfeng 0:8f759cbfc3c9 662 uint8_t cmd;
ryanfeng 0:8f759cbfc3c9 663 if( index == 0 ) {
ryanfeng 0:8f759cbfc3c9 664 cmd = 0x04;
ryanfeng 0:8f759cbfc3c9 665 } else if( index == 1 ) {
ryanfeng 0:8f759cbfc3c9 666 cmd = 0x07;
ryanfeng 0:8f759cbfc3c9 667 } else if( index == 2 ) {
ryanfeng 0:8f759cbfc3c9 668 cmd = 0x09;
ryanfeng 0:8f759cbfc3c9 669 } else {
ryanfeng 0:8f759cbfc3c9 670 return;
ryanfeng 0:8f759cbfc3c9 671 }
ryanfeng 0:8f759cbfc3c9 672
ryanfeng 0:8f759cbfc3c9 673 uint8_t top7;
ryanfeng 0:8f759cbfc3c9 674 uint8_t bottom7;
ryanfeng 0:8f759cbfc3c9 675
ryanfeng 0:8f759cbfc3c9 676 bottom7 = x0 & 0x7F;
ryanfeng 0:8f759cbfc3c9 677 top7 = (x0 & 0x3F80) >> 7;
ryanfeng 0:8f759cbfc3c9 678
ryanfeng 0:8f759cbfc3c9 679 maestro.putc(0xAA);
ryanfeng 0:8f759cbfc3c9 680 maestro.putc(0x0C);
ryanfeng 0:8f759cbfc3c9 681 maestro.putc(cmd);
ryanfeng 0:8f759cbfc3c9 682 maestro.putc(0x00);
ryanfeng 0:8f759cbfc3c9 683 maestro.putc(bottom7);
ryanfeng 0:8f759cbfc3c9 684 maestro.putc(top7);
ryanfeng 0:8f759cbfc3c9 685
ryanfeng 0:8f759cbfc3c9 686 bottom7 = x1 & 0x7F;
ryanfeng 0:8f759cbfc3c9 687 top7 = (x1 & 0x3F80) >> 7;
ryanfeng 0:8f759cbfc3c9 688
ryanfeng 0:8f759cbfc3c9 689 maestro.putc(0xAA);
ryanfeng 0:8f759cbfc3c9 690 maestro.putc(0x0C);
ryanfeng 0:8f759cbfc3c9 691 maestro.putc(cmd);
ryanfeng 0:8f759cbfc3c9 692 maestro.putc(0x01);
ryanfeng 0:8f759cbfc3c9 693 maestro.putc(bottom7);
ryanfeng 0:8f759cbfc3c9 694 maestro.putc(top7);
ryanfeng 0:8f759cbfc3c9 695
ryanfeng 0:8f759cbfc3c9 696 bottom7 = x2 & 0x7F;
ryanfeng 0:8f759cbfc3c9 697 top7 = (x2 & 0x3F80) >> 7;
ryanfeng 0:8f759cbfc3c9 698
ryanfeng 0:8f759cbfc3c9 699 maestro.putc(0xAA);
ryanfeng 0:8f759cbfc3c9 700 maestro.putc(0x0C);
ryanfeng 0:8f759cbfc3c9 701 maestro.putc(cmd);
ryanfeng 0:8f759cbfc3c9 702 maestro.putc(0x02);
ryanfeng 0:8f759cbfc3c9 703 maestro.putc(bottom7);
ryanfeng 0:8f759cbfc3c9 704 maestro.putc(top7);
ryanfeng 0:8f759cbfc3c9 705 }