Ryan Feng / Mbed 2 deprecated FishStateChart

StateChart defining behavior for FlippityFlappity project

Dependencies:   MMA8451Q Multi_WS2811 TSI mbed

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

Committer:
ryanfeng
Date:
Mon Apr 06 20:27:47 2015 +0000
Revision:
0:e72b30045834
Original StateChart for FlippityFlappity.  Find presentation statechart in FishStateChartDemo

Who changed what in which revision?

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