![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |