Ryan Feng
StateChart defining behavior for FlippityFlappity project
Dependencies: MMA8451Q Multi_WS2811 TSI mbed
Revision 0:e72b30045834, committed 2015-04-06
- Comitter:
- ryanfeng
- Date:
- Mon Apr 06 20:27:47 2015 +0000
- Commit message:
- Original StateChart for FlippityFlappity. Find presentation statechart in FishStateChartDemo
Changed in this revision
diff -r 000000000000 -r e72b30045834 MMA8451Q.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MMA8451Q.lib Mon Apr 06 20:27:47 2015 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/emilmont/code/MMA8451Q/#c4d879a39775
diff -r 000000000000 -r e72b30045834 Multi_WS2811.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Multi_WS2811.lib Mon Apr 06 20:27:47 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/ryanfeng/code/Multi_WS2811/#c4123ca9b02c
diff -r 000000000000 -r e72b30045834 TSI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TSI.lib Mon Apr 06 20:27:47 2015 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/TSI/#1a60ef257879
diff -r 000000000000 -r e72b30045834 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Apr 06 20:27:47 2015 +0000
@@ -0,0 +1,106 @@
+#include "mbed.h"
+#include "string"
+#include "list"
+#include "WS2811.h"
+#include "Colors.h"
+#include "TSISensor.h"
+#include "MMA8451Q.h"
+#include "mpr121.h"
+#include "statechart.h"
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+
+/*-----------------------------------------------------------------------*/
+
+// Neopixel Setup
+unsigned const nLEDs = MAX_LEDS_PER_STRIP; // Defined in WS2811.h
+unsigned const DATA_OUT_PIN1 = 2; // PTD2
+unsigned const DATA_OUT_PIN2 = 3; // PTD3
+
+// Maestro Setup
+Serial maestro(USBTX,USBRX);
+
+// Timer Setup
+Timer timeRunning;
+
+// MPR121 Setup
+InterruptIn interrupt(D9);
+I2C i2c(D14, D15);
+Mpr121 mpr121(&i2c, Mpr121::ADD_VSS); // constructor(i2c object, i2c address of the mpr121)
+int touch_val = 0;
+int prox_val = 0;
+
+// Accelerometer Setup
+MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
+
+// Battery Read Setup
+AnalogIn battery(A1);
+
+/*-----------------------------------------------------------------------*/
+
+void setToProxMode( bool prox ) {
+
+ if( prox ) { // Proximity Mode Setup
+
+ mpr121.setProximityMode(true);
+ unsigned char mode = mpr121.read(ELE_CFG); // Get the current mode
+ mpr121.write(ELE_CFG,0x00); // Put the MPR into setup mode
+ mpr121.write(EPROXTTH, 0x00); // Write the new threshold
+ mpr121.write(EPROXRTH, 0x00);
+ mpr121.write(ELE_CFG, mode); //Restore the operating mode
+
+ } else { // Touch Mode Setup
+
+ mpr121.setProximityMode(false);
+ unsigned char badTouch[12] = { 0x05, 0x05, 0x05, // Tail-side Body
+ 0x05, 0x05, 0x05, // Tail-side End
+ 0x07, 0x07, 0x07, // Head-side Body
+ 0x05, 0x05, 0x05,}; // Head-side End
+
+ unsigned char sweetRelease[12] = { 0x02, 0x02, 0x02, // Tail-side Body
+ 0x02, 0x02, 0x02, // Tail-side End
+ 0x02, 0x02, 0x02, // Head-side Body
+ 0x02, 0x02, 0x02}; // Head-side End
+ for( int i = 0; i < 12; i++ ) {
+ mpr121.setElectrodeThreshold(i,badTouch[i],sweetRelease[i]); // Reset Touch Sensitivity
+ }
+ }
+}
+
+// Primary Interrupt Function for MPR121
+void fallInterrupt() {
+ touch_val = mpr121.read(0x00);
+ touch_val += mpr121.read(0x01) << 8;
+}
+
+/*-----------------------------------------------------------------------*/
+
+int main() {
+
+ WS2811 lightStrip1(nLEDs, DATA_OUT_PIN1);
+ WS2811 lightStrip2(nLEDs, DATA_OUT_PIN2);
+ lightStrip1.begin();
+ lightStrip2.begin();
+
+ Statechart statechart;
+
+ // Set to Touch Mode
+ setToProxMode(false);
+
+ // Start MPR121 Interrupts
+ interrupt.fall(&fallInterrupt);
+ interrupt.mode(PullUp);
+
+ // Main Loop: Show Neopixels
+ timeRunning.start();
+ float currTime;
+ float xyz[3];
+
+ while(true) {
+ currTime = timeRunning.read_ms();
+ acc.getAccAllAxis(xyz);
+
+ statechart.fishStatechart(currTime, touch_val, prox_val, xyz, maestro, lightStrip1, lightStrip2, battery);
+ }
+}
+
diff -r 000000000000 -r e72b30045834 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Apr 06 20:27:47 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/4fc01daae5a5 \ No newline at end of file
diff -r 000000000000 -r e72b30045834 mpr121.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mpr121.cpp Mon Apr 06 20:27:47 2015 +0000
@@ -0,0 +1,221 @@
+/*
+Copyright (c) 2011 Anthony Buckton (abuckton [at] blackink [dot} net {dot} au)
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*/
+
+#include <mbed.h>
+#include <sstream>
+#include <string>
+#include <list>
+
+#include <mpr121.h>
+
+Mpr121::Mpr121(I2C *i2c, Address i2cAddress)
+{
+ this->i2c = i2c;
+
+ address = i2cAddress;
+
+ // Configure the MPR121 settings to default
+ this->configureSettings();
+}
+
+
+void Mpr121::configureSettings()
+{
+ // Put the MPR into setup mode
+ this->write(ELE_CFG,0x00);
+
+ // Electrode filters for when data is > baseline
+ unsigned char gtBaseline[] = {
+ 0x01, //MHD_R
+ 0x01, //NHD_R
+ 0x00, //NCL_R
+ 0x00 //FDL_R
+ };
+
+ writeMany(MHD_R,gtBaseline,4);
+
+ // Electrode filters for when data is < baseline
+ unsigned char ltBaseline[] = {
+ 0x01, //MHD_F
+ 0x01, //NHD_F
+ 0xFF, //NCL_F
+ 0x02 //FDL_F
+ };
+
+ writeMany(MHD_F,ltBaseline,4);
+
+ // Electrode touch and release thresholds
+ unsigned char electrodeThresholds[] = {
+ E_THR_T, // Touch Threshhold
+ E_THR_R // Release Threshold
+ };
+
+ for(int i=0; i<12; i++){
+ int result = writeMany((ELE0_T+(i*2)),electrodeThresholds,2);
+ }
+
+ // Proximity Settings
+ unsigned char proximitySettings[] = {
+ 0xff, //MHD_Prox_R
+ 0xff, //NHD_Prox_R
+ 0x00, //NCL_Prox_R
+ 0x00, //FDL_Prox_R
+ 0x01, //MHD_Prox_F
+ 0x01, //NHD_Prox_F
+ 0xFF, //NCL_Prox_F
+ 0xff, //FDL_Prox_F
+ 0x00, //NHD_Prox_T
+ 0x00, //NCL_Prox_T
+ 0x00 //NFD_Prox_T
+ };
+ writeMany(MHDPROXR,proximitySettings,11);
+
+ unsigned char proxThresh[] = {
+ PROX_THR_T, // Touch Threshold
+ PROX_THR_R // Release Threshold
+ };
+ writeMany(EPROXTTH,proxThresh,2);
+
+ this->write(FIL_CFG,0x04);
+
+ // Set the electrode config to transition to active mode
+ this->write(ELE_CFG,0x0c);
+}
+
+void Mpr121::setElectrodeThreshold(int electrode, unsigned char touch, unsigned char release){
+
+ if(electrode > 11) return;
+
+ // Get the current mode
+ unsigned char mode = this->read(ELE_CFG);
+
+ // Put the MPR into setup mode
+ this->write(ELE_CFG,0x00);
+
+ // Write the new threshold
+ this->write((ELE0_T+(electrode*2)), touch);
+ this->write((ELE0_T+(electrode*2)+1), release);
+
+ //Restore the operating mode
+ this->write(ELE_CFG, mode);
+}
+
+
+unsigned char Mpr121::read(int key){
+
+ unsigned char data[2];
+
+ //Start the command
+ i2c->start();
+
+ // Address the target (Write mode)
+ int ack1= i2c->write(address);
+
+ // Set the register key to read
+ int ack2 = i2c->write(key);
+
+ // Re-start for read of data
+ i2c->start();
+
+ // Re-send the target address in read mode
+ int ack3 = i2c->write(address+1);
+
+ // Read in the result
+ data[0] = i2c->read(0);
+
+ // Reset the bus
+ i2c->stop();
+
+ return data[0];
+}
+
+
+int Mpr121::write(int key, unsigned char value){
+
+ //Start the command
+ i2c->start();
+
+ // Address the target (Write mode)
+ int ack1= i2c->write(address);
+
+ // Set the register key to write
+ int ack2 = i2c->write(key);
+
+ // Read in the result
+ int ack3 = i2c->write(value);
+
+ // Reset the bus
+ i2c->stop();
+
+ return (ack1+ack2+ack3)-3;
+}
+
+
+int Mpr121::writeMany(int start, unsigned char* dataSet, int length){
+ //Start the command
+ i2c->start();
+
+ // Address the target (Write mode)
+ int ack= i2c->write(address);
+ if(ack!=1){
+ return -1;
+ }
+
+ // Set the register key to write
+ ack = i2c->write(start);
+ if(ack!=1){
+ return -1;
+ }
+
+ // Write the date set
+ int count = 0;
+ while(ack==1 && (count < length)){
+ ack = i2c->write(dataSet[count]);
+ count++;
+ }
+ // Stop the cmd
+ i2c->stop();
+
+ return count;
+}
+
+
+bool Mpr121::getProximityMode(){
+ if(this->read(ELE_CFG) > 0x0c)
+ return true;
+ else
+ return false;
+}
+
+void Mpr121::setProximityMode(bool mode){
+ this->write(ELE_CFG,0x00);
+ if(mode){
+ this->write(ELE_CFG,0x30); //Sense proximity from ALL pads
+ } else {
+ this->write(ELE_CFG,0x0c); //Sense touch, all 12 pads active.
+ }
+}
+
+
+int Mpr121::readTouchData(){
+ return this->read(0x00);
+}
\ No newline at end of file
diff -r 000000000000 -r e72b30045834 mpr121.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/mpr121.h Mon Apr 06 20:27:47 2015 +0000
@@ -0,0 +1,157 @@
+/*
+Copyright (c) 2011 Anthony Buckton (abuckton [at] blackink [dot} net {dot} au)
+
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+
+ Parts written by Jim Lindblom of Sparkfun
+ Ported to mbed by A.Buckton, Feb 2011
+*/
+
+#ifndef MPR121_H
+#define MPR121_H
+
+//using namespace std;
+
+class Mpr121
+{
+
+public:
+ // i2c Addresses, bit-shifted
+ enum Address { ADD_VSS = 0xb4,// ADD->VSS = 0x5a <-wiring on Sparkfun board
+ ADD_VDD = 0xb6,// ADD->VDD = 0x5b
+ ADD_SCL = 0xb8,// ADD->SDA = 0x5c
+ ADD_SDA = 0xba // ADD->SCL = 0x5d
+ };
+
+ // Real initialiser, takes the i2c address of the device.
+ Mpr121(I2C *i2c, Address i2cAddress);
+
+ bool getProximityMode();
+
+ void setProximityMode(bool mode);
+
+ int readTouchData();
+
+ unsigned char read(int key);
+
+ int write(int address, unsigned char value);
+ int writeMany(int start, unsigned char* dataSet, int length);
+
+ void setElectrodeThreshold(int electrodeId, unsigned char touchThreshold, unsigned char releaseThreshold);
+
+protected:
+ // Configures the MPR with standard settings. This is permitted to be overwritten by sub-classes.
+ void configureSettings();
+
+private:
+ // The I2C bus instance.
+ I2C *i2c;
+
+ // i2c address of this mpr121
+ Address address;
+};
+
+
+// MPR121 Register Defines
+#define MHD_R 0x2B
+#define NHD_R 0x2C
+#define NCL_R 0x2D
+#define FDL_R 0x2E
+#define MHD_F 0x2F
+#define NHD_F 0x30
+#define NCL_F 0x31
+#define FDL_F 0x32
+#define NHDT 0x33
+#define NCLT 0x34
+#define FDLT 0x35
+// Proximity sensing controls
+#define MHDPROXR 0x36
+#define NHDPROXR 0x37
+#define NCLPROXR 0x38
+#define FDLPROXR 0x39
+#define MHDPROXF 0x3A
+#define NHDPROXF 0x3B
+#define NCLPROXF 0x3C
+#define FDLPROXF 0x3D
+#define NHDPROXT 0x3E
+#define NCLPROXT 0x3F
+#define FDLPROXT 0x40
+// Electrode Touch/Release thresholds
+#define ELE0_T 0x41
+#define ELE0_R 0x42
+#define ELE1_T 0x43
+#define ELE1_R 0x44
+#define ELE2_T 0x45
+#define ELE2_R 0x46
+#define ELE3_T 0x47
+#define ELE3_R 0x48
+#define ELE4_T 0x49
+#define ELE4_R 0x4A
+#define ELE5_T 0x4B
+#define ELE5_R 0x4C
+#define ELE6_T 0x4D
+#define ELE6_R 0x4E
+#define ELE7_T 0x4F
+#define ELE7_R 0x50
+#define ELE8_T 0x51
+#define ELE8_R 0x52
+#define ELE9_T 0x53
+#define ELE9_R 0x54
+#define ELE10_T 0x55
+#define ELE10_R 0x56
+#define ELE11_T 0x57
+#define ELE11_R 0x58
+// Proximity Touch/Release thresholds
+#define EPROXTTH 0x59
+#define EPROXRTH 0x5A
+// Debounce configuration
+#define DEB_CFG 0x5B
+// AFE- Analogue Front End configuration
+#define AFE_CFG 0x5C
+// Filter configuration
+#define FIL_CFG 0x5D
+// Electrode configuration - transistions to "active mode"
+#define ELE_CFG 0x5E
+
+#define GPIO_CTRL0 0x73
+#define GPIO_CTRL1 0x74
+#define GPIO_DATA 0x75
+#define GPIO_DIR 0x76
+#define GPIO_EN 0x77
+#define GPIO_SET 0x78
+#define GPIO_CLEAR 0x79
+#define GPIO_TOGGLE 0x7A
+// Auto configration registers
+#define AUTO_CFG_0 0x7B
+#define AUTO_CFG_U 0x7D
+#define AUTO_CFG_L 0x7E
+#define AUTO_CFG_T 0x7F
+
+// Threshold defaults
+// Electrode touch threshold
+#define E_THR_T 0x0F
+// Electrode release threshold
+#define E_THR_R 0x0A
+// Prox touch threshold
+#define PROX_THR_T 0x02
+// Prox release threshold
+#define PROX_THR_R 0x02
+
+#endif
diff -r 000000000000 -r e72b30045834 statechart.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/statechart.cpp Mon Apr 06 20:27:47 2015 +0000
@@ -0,0 +1,805 @@
+#include <mbed.h>
+
+#include "WS2811.h"
+#include "Colors.h"
+
+#include <cmath>
+#include <statechart.h>
+
+/* TODO LIST
+ [!] FSM for behavior
+ [!] Simplify FSM for practicality
+ [!] Write State Transitions
+ [!] Validate Transitions
+ [!] Validate Speeds, Accels
+ [!] Write State Actions
+ [!] Test LOOK_LEFT action
+ [!] Test LOOK_RIGHT action
+ [!] Test FLOP action
+ [ ] Test WALK action
+ [!] Test FLAP action
+*/
+
+void Statechart::fishStatechart(int netTime, int capNum, int prox, float* xyz, Serial& maestro, WS2811& lightStrip1, WS2811& lightStrip2, float battery) {
+ static actState_t actState = REST;
+ static emoState_t emoState = CURIOUS;
+ static int timeAtManeuverStart = 0;
+
+ /*
+ // IMPORTANT: NEVER RUN THE LIPO BATTERIES BELOW 80% OF THEIR MAX CAPACITY
+ if( actState != BATTERY_LOW && battery < 0.57 ) {
+ actState = BATTERY_LOW;
+ timeAtManeuverStart = netTime;
+ }*/
+
+ bool cap[12] = {false,false,false,
+ false,false,false,
+ false,false,false,
+ false,false,false};
+ int i;
+ int temp = capNum;
+ bool touched = false;
+ for (i=0; i<12; i++) {
+ if (((temp>>i)&0x01)==1) {
+ cap[i] = true;
+ touched = true;
+ }
+ }
+
+ bool trans = (( rand() % 100 ) == 0);
+ int randNum;
+ uint16_t p0, p1, p2;
+
+ // state transitions
+ switch(actState) {
+ case REST:
+ switch(emoState) {
+ case CURIOUS:
+ //if touched REST & HAPPY
+ if( touched ) {
+ emoState = HAPPY;
+ }
+ //if !touched REST & MAD
+ //LOOK_LEFT, LOOK_RIGHT, WALK
+ else if( trans ) {
+ randNum = rand() % 4;
+ if( randNum == 0 ) {
+ actState = LOOK_LEFT;
+ timeAtManeuverStart = netTime;
+ } else if( randNum == 1 ) {
+ actState = LOOK_RIGHT;
+ timeAtManeuverStart = netTime;
+ } else if( randNum == 2 ) {
+ actState = WALK;
+ timeAtManeuverStart = netTime;
+ } else if( randNum == 3 ) {
+ emoState = MAD;
+ timeAtManeuverStart = netTime;
+ }
+ }
+ break;
+ case MAD:
+ //if touched back LOOK_LEFT
+ if( cap[0] || cap[3] || cap[6] || cap[9] ) {
+ actState = LOOK_LEFT;
+ timeAtManeuverStart = netTime;
+ }
+ //if touched front LOOK_RIGHT
+ else if( cap[1] || cap[4] || cap[7] || cap[10] ) {
+ actState = LOOK_RIGHT;
+ timeAtManeuverStart = netTime;
+ }
+ //FLAP
+ else if( trans && (netTime - timeAtManeuverStart) > 1500 ) {
+ actState = FLAP;
+ timeAtManeuverStart = netTime;
+ }
+ break;
+ case HAPPY:
+ //if touched FLAP
+ if( touched ) {
+ actState = FLAP;
+ timeAtManeuverStart = netTime;
+ }
+ //FLOP or REST & CURIOUS
+ else if( trans ) {
+ randNum = rand() % 3;
+ if( randNum < 2 ) {
+ actState = FLOP;
+ timeAtManeuverStart = netTime;
+ } else if( randNum == 2 ) {
+ actState = REST;
+ emoState = CURIOUS;
+ timeAtManeuverStart = netTime;
+ }
+ }
+ break;
+ case SAD:
+ //FLOP
+ if( trans ) {
+ actState = FLOP;
+ timeAtManeuverStart = netTime;
+ }
+ break;
+ } //switch(emoState) {
+ break; //case REST:
+ case LOOK_LEFT:
+ switch(emoState) {
+ case CURIOUS:
+ //REST, LOOK_RIGHT
+ if( trans || netTime - timeAtManeuverStart > 3000 ) {
+ randNum = rand() % 2;
+ if( randNum == 0 ) {
+ actState = REST;
+ timeAtManeuverStart = netTime;
+ } else if( randNum == 1 ) {
+ actState = LOOK_RIGHT;
+ timeAtManeuverStart = netTime;
+ }
+ }
+ break;
+ case MAD: //HEY
+ //REST
+ if( trans && netTime - timeAtManeuverStart > 3000 ) {
+ actState = REST;
+ timeAtManeuverStart = netTime;
+ }
+ break;
+ case HAPPY: //Nudge
+ //if touched FLOP
+ if( touched ) {
+ actState = FLOP;
+ timeAtManeuverStart = netTime;
+ }
+ //if !touched REST & SAD
+ //FLOP
+ else if( trans && netTime - timeAtManeuverStart > 3000 ) {
+ randNum = rand() % 2;
+ if( randNum == 0 ) {
+ actState = REST;
+ emoState = SAD;
+ timeAtManeuverStart = netTime;
+ }
+ else if( randNum == 1 ) {
+ actState = FLOP;
+ timeAtManeuverStart = netTime;
+ }
+ }
+ break;
+ case SAD:
+ //
+ break;
+ } //switch(emoState) {
+ break; //case LOOK_LEFT:
+ case LOOK_RIGHT:
+ switch(emoState) {
+ case CURIOUS:
+ //REST, LOOK_RIGHT
+ if( trans || netTime - timeAtManeuverStart > 3000 ) {
+ randNum = rand() % 2;
+ if( randNum == 0 ) {
+ actState = REST;
+ timeAtManeuverStart = netTime;
+ } else if ( randNum == 1 ) {
+ actState = LOOK_LEFT;
+ timeAtManeuverStart = netTime;
+ }
+ }
+ break;
+ case MAD: //HEY
+ //REST
+ if( trans && netTime - timeAtManeuverStart > 3000 ) {
+ actState = REST;
+ timeAtManeuverStart = netTime;
+ }
+ break;
+ case HAPPY: //Nudge
+ //if touched FLOP
+ if( touched ) {
+ actState = FLOP;
+ timeAtManeuverStart = netTime;
+ }
+ //if !touched REST & SAD or REST & MAD
+ //FLOP
+ else if( trans && netTime - timeAtManeuverStart > 3000 ) {
+ randNum = rand() % 2;
+ if( randNum == 0 ) {
+ randNum = rand() % 2;
+ if( randNum == 0 ) {
+ actState = REST;
+ emoState = SAD;
+ timeAtManeuverStart = netTime;
+ } else if( randNum == 1 ) {
+ actState = REST;
+ emoState = MAD;
+ timeAtManeuverStart = netTime;
+ }
+ }
+ else if( randNum == 1 ) {
+ actState = FLOP;
+ timeAtManeuverStart = netTime;
+ }
+ }
+ break;
+ case SAD:
+ //
+ break;
+ } //switch(emoState) {
+ break; //case LOOK_RIGHT:
+ case FLOP:
+ switch(emoState) {
+ case CURIOUS:
+ //
+ break;
+ case MAD:
+ //
+ break;
+ case HAPPY: //Hey!
+ //if touched WALK, FLAP
+ if( touched ) {
+ randNum = rand() % 2;
+ if( randNum == 0 ) {
+ actState = WALK;
+ timeAtManeuverStart = netTime;
+ }
+ else if( randNum == 1 ) {
+ actState = FLAP;
+ timeAtManeuverStart = netTime;
+ }
+ }
+ //REST, LOOK_LEFT, LOOK_RIGHT
+ else if( trans && netTime - timeAtManeuverStart > 500 ) {
+ randNum = rand() % 3;
+ if( randNum == 0 ) {
+ actState = REST;
+ timeAtManeuverStart = netTime;
+ } else if( randNum == 1 ) {
+ actState = LOOK_LEFT;
+ timeAtManeuverStart = netTime;
+ } else if( randNum == 2 ) {
+ actState = LOOK_RIGHT;
+ timeAtManeuverStart = netTime;
+ }
+ }
+ break;
+ case SAD: //Sigh...
+ //if touched REST & HAPPY
+ if( touched ) {
+ actState = REST;
+ emoState = HAPPY;
+ timeAtManeuverStart = netTime;
+ }
+ //REST, FLAP
+ else if( trans && netTime - timeAtManeuverStart > 1000 ) {
+ randNum = rand() % 2;
+ if( randNum == 0 ) {
+ actState = REST;
+ timeAtManeuverStart = netTime;
+ } else if( randNum == 1) {
+ actState = FLAP;
+ timeAtManeuverStart = netTime;
+ }
+ }
+ break;
+ } //switch(emoState) {
+ break; //case FLOP:
+ case WALK:
+ switch(emoState) {
+ case CURIOUS: //Search
+ //if touched REST & HAPPY
+ if( touched ) {
+ actState = REST;
+ emoState = HAPPY;
+ timeAtManeuverStart = netTime;
+ }
+ //if !touched REST & MAD
+ //REST
+ else if( trans && netTime - timeAtManeuverStart > 3000 ) {
+ randNum = rand() % 3;
+ if( randNum == 2 ) {
+ actState = REST;
+ emoState = MAD;
+ timeAtManeuverStart = netTime;
+ } else if( randNum < 2 ) {
+ actState = REST;
+ timeAtManeuverStart = netTime;
+ }
+ }
+ break;
+ case MAD:
+ //
+ break;
+ case HAPPY: //Chase
+ //FLOP
+ if( trans && netTime - timeAtManeuverStart > 3000 ) {
+ actState = FLOP;
+ timeAtManeuverStart = netTime;
+ }
+ break;
+ case SAD:
+ //
+ break;
+ } //switch(emoState) {
+ break; //case WALK:
+ case FLAP:
+ switch(emoState) {
+ case CURIOUS:
+ //
+ break;
+ case MAD: //Tantrum
+ //if touched REST & HAPPY
+ if( touched ) {
+ actState = REST;
+ emoState = HAPPY;
+ }
+ break;
+ case HAPPY: //Laugh
+ //if touched FLAP
+ if( touched ) {
+ timeAtManeuverStart = netTime;
+ }
+ //REST
+ else if( trans && netTime - timeAtManeuverStart > 750 ) {
+ actState = REST;
+ timeAtManeuverStart = netTime;
+ }
+ break;
+ case SAD: //Sleep
+ //if touched REST & HAPPY
+ if( touched ) {
+ actState = REST;
+ emoState = HAPPY;
+ timeAtManeuverStart = netTime;
+ }
+ break;
+ } //switch(emoState) {
+ break; //case FLAP:
+ case BATTERY_LOW:
+ break;
+ } //switch(actState) {
+
+ // define what states do
+ switch(actState){
+ case REST:
+ switch( emoState ) {
+ case CURIOUS:
+ showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+ showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+ setServoValue(maestro, 1, 50, 50, 50);
+ setServoValue(maestro, 2, 0, 0, 0);
+ p0 = 6200;
+ p1 = 6200;
+ p2 = 6200;
+ break;
+ case MAD:
+ if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) {
+ showSolidColorBright(lightStrip1, 255, 0, 0, 0.5);
+ showSolidColorBright(lightStrip2, 255, 0, 0, 0.5);
+ } else {
+ showSolidColorBright(lightStrip1, 255, 0, 0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
+ showSolidColorBright(lightStrip2, 255, 0, 0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
+ }
+ setServoValue(maestro, 1, 60, 60, 60);
+ setServoValue(maestro, 2, 30, 30, 30);
+ p0 = 6200;
+ p1 = 6200;
+ p2 = 6200;
+ break;
+ case HAPPY:
+ showRainbow(lightStrip1, 1.0, MINBRITE + (MAXBRITE - MINBRITE) * abs(xyz[0]), 0.5 + 0.5 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000));
+ showRainbow(lightStrip2, 1.0, MINBRITE + (MAXBRITE - MINBRITE) * abs(xyz[1]), 0.5 + 0.5 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000));
+ setServoValue(maestro, 1, 100, 100, 100);
+ setServoValue(maestro, 2, 0, 0, 0);
+ p0 = 6200;
+ p1 = 6200;
+ p2 = 6200;
+ break;
+ case SAD:
+ if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) {
+ showSolidColorBright(lightStrip1, 0, 0, 255, 0.2);
+ showSolidColorBright(lightStrip2, 0, 0, 255, 0.2);
+ } else {
+ showSolidColorBright(lightStrip1, 0, 0, 255, (float)(0.35 - 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
+ showSolidColorBright(lightStrip2, 0, 0, 255, (float)(0.35 - 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000)));
+ }
+ setServoValue(maestro, 1, 20, 20, 20);
+ setServoValue(maestro, 2, 0, 0, 0);
+ p0 = 6200;
+ p1 = 6200;
+ p2 = 6200;
+ break;
+ } //switch(emoState) {
+ WS2811::startDMA();
+ setServoValue(maestro, 0, p0, p1, p2);
+ break; //case REST:
+ case LOOK_LEFT:
+ switch( emoState ) {
+ case CURIOUS: //Search >.>
+ showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.4 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
+ showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.3 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
+ setServoValue(maestro, 1, 50, 50, 50);
+ setServoValue(maestro, 2, 0, 0, 0);
+ if( netTime - timeAtManeuverStart > 3000 ) {
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ } else {
+ p0 = 3000;
+ p1 = 8000;
+ p2 = 6200;
+ }
+ break;
+ case MAD: //HEY >:(
+ showSolidColorBright(lightStrip1, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
+ showSolidColorBright(lightStrip2, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
+ setServoValue(maestro, 1, 100, 100, 100);
+ setServoValue(maestro, 2, 0, 0, 0);
+ if( netTime - timeAtManeuverStart > 3000 ) {
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ } else {
+ p0 = 3000;
+ p1 = 8000;
+ p2 = 6200;
+ }
+ break;
+ case HAPPY: //Nudge '.^
+ showSolidColorBright(lightStrip1, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));
+ showSolidColorBright(lightStrip2, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));
+ setServoValue(maestro, 1, 70, 70, 70);
+ setServoValue(maestro, 2, 0, 0, 0);
+ if( netTime - timeAtManeuverStart > 3000 ) {
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ } else {
+ p0 = 3000;
+ p1 = 8000;
+ p2 = 6200;
+ }
+ break;
+ case SAD: //Unreachable state O.O
+ showSolidColor(lightStrip1, 0, 0, 0);
+ showSolidColor(lightStrip2, 0, 0, 0);
+ setServoValue(maestro, 1, 10, 10, 10);
+ setServoValue(maestro, 2, 10, 10, 10);
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ break;
+ } //switch(emoState) {
+ WS2811::startDMA();
+ setServoValue(maestro, 0, p0, p1, p2);
+ break; //case LOOK_LEFT:
+ case LOOK_RIGHT:
+ switch( emoState ) {
+ case CURIOUS: //Search <.<
+ showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.4 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
+ showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.3 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000)));
+ setServoValue(maestro, 1, 50, 50, 50);
+ setServoValue(maestro, 2, 0, 0, 0);
+ if( netTime - timeAtManeuverStart > 3000 ) {
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ } else {
+ p0 = 5000;
+ p1 = 10000;
+ p2 = 6200;
+ }
+ break;
+ case MAD: //HEY >:(
+ showSolidColorBright(lightStrip1, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
+ showSolidColorBright(lightStrip2, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000))));
+ setServoValue(maestro, 1, 120, 120, 120);
+ setServoValue(maestro, 2, 0, 0, 0);
+ if( netTime - timeAtManeuverStart > 3000 ) {
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ } else {
+ p0 = 5000;
+ p1 = 10000;
+ p2 = 6200;
+ }
+ break;
+ case HAPPY: //Nudge '.^
+ showSolidColorBright(lightStrip1, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));
+ showSolidColorBright(lightStrip2, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500)));
+ setServoValue(maestro, 1, 70, 70, 70);
+ setServoValue(maestro, 2, 0, 0, 0);
+ if( netTime - timeAtManeuverStart > 3000 ) {
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ } else {
+ p0 = 5000;
+ p1 = 10000;
+ p2 = 6200;
+ }
+ break;
+ case SAD: //Unreachable state O.O
+ showSolidColor(lightStrip1, 255, 255, 255);
+ showSolidColor(lightStrip2, 255, 255, 255);
+ setServoValue(maestro, 1, 10, 10, 10);
+ setServoValue(maestro, 2, 10, 10, 10);
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ break;
+ } //switch(emoState) {
+ WS2811::startDMA();
+ setServoValue(maestro, 0, p0, p1, p2);
+ break; //case LOOK_RIGHT
+ case FLOP:
+ switch( emoState ) {
+ case CURIOUS: //Unreachable state O.O
+ showSolidColor(lightStrip1, 0, 0, 0);
+ showSolidColor(lightStrip2, 0, 0, 0);
+ setServoValue(maestro, 1, 50, 50, 50);
+ setServoValue(maestro, 2, 0, 0, 0);
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ break;
+ case MAD: //Unreachable state O.O
+ showSolidColor(lightStrip1, 0, 0, 0);
+ showSolidColor(lightStrip2, 0, 0, 0);
+ setServoValue(maestro, 1, 50, 50, 50);
+ setServoValue(maestro, 2, 50, 50, 50);
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ break;
+ case HAPPY: //Hey! :D
+ if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) {
+ showRainbow(lightStrip1, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/500)), abs(xyz[0]));
+ showRainbow(lightStrip2, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/500)), abs(xyz[1]));
+ } else {
+ showRainbow(lightStrip1, 1.0, 0.2, abs(xyz[0]));
+ showRainbow(lightStrip2, 1.0, 0.2, abs(xyz[1]));
+ }
+ setServoValue(maestro, 1, 80, 80, 80);
+ setServoValue(maestro, 2, 0, 0, 0);
+ p0 = 4000;
+ if( (netTime - timeAtManeuverStart) % 4000 > 2000 ) {
+ p1 = 6200;
+ p2 = 6200;
+ } else {
+ p1 = 2500;
+ p2 = 2500;
+ }
+ break;
+ case SAD: //Sigh... :/
+ showSolidColorBright(lightStrip1, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+ showSolidColorBright(lightStrip2, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+ setServoValue(maestro, 1, 20, 20, 20);
+ setServoValue(maestro, 2, 10, 10, 10);
+ p0 = 4000;
+ if( (netTime - timeAtManeuverStart) % 4000 > 1000 ) {
+ p1 = 6200;
+ p2 = 6200;
+ } else {
+ p1 = 2500;
+ p2 = 2500;
+ }
+ break;
+ } //switch(emoState) {
+ WS2811::startDMA();
+ setServoValue(maestro, 0, p0, p1, p2);
+ break; //case FLOP:
+ case WALK:
+ switch( emoState ) {
+ case CURIOUS: //Search o.o
+ if( (netTime - timeAtManeuverStart) % 3000 > 2000 ) {
+ showSomeRainbow(lightStrip1, 0.45 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 0.82 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+ showSomeRainbow(lightStrip2, 0.45 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 0.82 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+ } else {
+ showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+ showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+ }
+ setServoValue(maestro, 1, 70, 70, 70);
+ setServoValue(maestro, 2, 0, 0, 0);
+ p0 = (int)(7000 + 3000 * (0.5 * cos(4*PI*(double)(netTime - timeAtManeuverStart)/5000 + 0*2*PI/3)));
+ p1 = (int)(5000 - 3000 * (0.5 * cos(4*PI*(double)(netTime - timeAtManeuverStart)/5000 + 1*2*PI/3)));
+ p2 = (int)(7000 + 3000 * (0.5 * cos(4*PI*(double)(netTime - timeAtManeuverStart)/5000 + 2*2*PI/3)));
+ break;
+ case MAD: //Unreachable state O.O
+ showSolidColor(lightStrip1, 0, 0, 0);
+ showSolidColor(lightStrip2, 0, 0, 0);
+ setServoValue(maestro, 1, 50, 50, 50);
+ setServoValue(maestro, 2, 50, 50, 50);
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ setServoValue(maestro, 0, 6000, 6000, 6000);
+ break;
+ case HAPPY: //Chase XD
+ showRainbow(lightStrip1, 1.0, 0.5, (float)((netTime - timeAtManeuverStart) % 500) / 500.0);
+ showRainbow(lightStrip2, 1.0, 0.5, (float)((netTime - timeAtManeuverStart) % 500) / 500.0);
+ setServoValue(maestro, 1, 100, 100, 100);
+ setServoValue(maestro, 2, 0, 0, 0);
+ p0 = (int)(7000 + 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 0*2*PI/3)));
+ p1 = (int)(5000 - 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 1*2*PI/3)));
+ p2 = (int)(7000 + 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 2*2*PI/3)));
+ break;
+ case SAD: //Unreachable state O.O
+ showSolidColor(lightStrip1, 0, 0, 0);
+ showSolidColor(lightStrip2, 0, 0, 0);
+ setServoValue(maestro, 1, 10, 10, 10);
+ setServoValue(maestro, 2, 10, 10, 10);
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ break;
+ } //switch(emoState) {
+ WS2811::startDMA();
+ setServoValue(maestro, 0, p0, p1, p2);
+ break; //case WALK:
+ case FLAP:
+ switch( emoState ) {
+ case CURIOUS: //Unreachable state O.O
+ showSolidColor(lightStrip1, 0, 0, 0);
+ showSolidColor(lightStrip2, 0, 0, 0);
+ setServoValue(maestro, 1, 50, 50, 50);
+ setServoValue(maestro, 2, 0, 0, 0);
+ p0 = 6000;
+ p1 = 6000;
+ p2 = 6000;
+ break;
+ case MAD: //Tantrum >:X
+ showSolidColorBright(lightStrip1, 153, 0, 0, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750)));
+ showSolidColorBright(lightStrip2, 153, 0, 0, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750)));
+ setServoValue(maestro, 1, 100, 100, 100);
+ setServoValue(maestro, 2, 0, 0, 0);
+ p0 = 4000;
+ p1 = 6200 - 7400 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750);
+ p2 = 6200 - 7400 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750);
+ break;
+ case HAPPY: //Laugh ^.^
+ showRainbow(lightStrip1, 1.0, 0.35, (float)((netTime - timeAtManeuverStart) % 1000) / 1000.0);
+ showRainbow(lightStrip2, 1.0, 0.35, (float)((netTime - timeAtManeuverStart) % 1000) / 1000.0);
+ setServoValue(maestro, 1, 100, 100, 100);
+ setServoValue(maestro, 2, 0, 0, 0);
+ p0 = 4000;
+ p1 = 6200;
+ p2 = 3600 - 800 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/500);
+ break;
+ case SAD: //Sleep -_-
+ showSolidColorBright(lightStrip1, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+ showSolidColorBright(lightStrip2, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000)));
+ setServoValue(maestro, 1, 20, 20, 20);
+ setServoValue(maestro, 2, 0, 0, 0);
+ p0 = 4000;
+ p1 = 6200;
+ p2 = 3200 - 1600 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000);
+ break;
+ } //switch(emoState) {
+ WS2811::startDMA();
+ setServoValue(maestro, 0, p0, p1, p2);
+ break; //case FLAP:
+ case BATTERY_LOW:
+ if( (netTime - timeAtManeuverStart) % 1000 > 500 ) {
+ showSolidColor(lightStrip1, 255, 0, 0);
+ showSolidColor(lightStrip2, 0, 0, 0);
+ } else {
+ showSolidColor(lightStrip1, 0, 0, 0);
+ showSolidColor(lightStrip2, 255, 0, 0);
+ }
+ WS2811::startDMA();
+ break;
+ } //switch(actState) {
+} //void Statechart::fishStateChart
+
+// @brief sets different colors in each of the LEDs of a strip
+// @param strip the light strip
+// @param sat saturation, 0.0 - 1.0
+// @param brite brightness, 0.0 - 1.0
+// @param hueShift shift, 0.0 - 1.0 is equivalent to 0 - 360 degrees
+void Statechart::showRainbow(WS2811 &strip, float sat, float brite, float hueShift)
+{
+ unsigned nLEDs = strip.numPixels();
+ for (unsigned i = 0; i < nLEDs; i++) {
+ uint8_t r, g, b;
+ float hue = ((float)i / (float)nLEDs) + hueShift;
+ HSBtoRGB(hue, sat, brite, &r, &g, &b);
+ strip.setPixelColor(i, r, g, b);
+ }
+ strip.show();
+}
+
+void Statechart::showSolidColor(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b)
+{
+ unsigned nLEDs = strip.numPixels();
+ for (unsigned i = 0; i < nLEDs; i++) {
+ strip.setPixelColor(i, r, g, b);
+ }
+ strip.show();
+}
+
+
+//showSolidColorBright(lightStrip1, 0, 0, 255, (float)(0.35 + 0.15 * sin(2*PI * (double)currTime / 1000)));
+//showSolidColorBright(lightStrip2, 0, 0, 255, (float)(0.35 + 0.15 * sin(2*PI * (double)currTime / 1000)));
+//WS2811::startDMA();
+void Statechart::showSolidColorBright(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b, float newBrite)
+{
+ unsigned nLEDs = strip.numPixels();
+ for (unsigned i = 0; i < nLEDs; i++) {
+ float hsbVals[3];
+ RGBtoHSB(r, g, b, hsbVals);
+ HSBtoRGB(hsbVals[0], hsbVals[1], newBrite, &r, &g, &b);
+ strip.setPixelColor(i, r, g, b);
+ }
+ strip.show();
+}
+
+void Statechart::showSomeRainbow(WS2811 &strip, float hueLow, float hueHigh, float sat, float brite)
+{
+ unsigned nLEDs = strip.numPixels();
+ for (unsigned i = 0; i < nLEDs; i++) {
+ uint8_t r, g, b;
+ float hue = hueLow + (hueHigh - hueLow) * ((float)i / (float)nLEDs);
+ HSBtoRGB(hue, sat, brite, &r, &g, &b);
+ strip.setPixelColor(i, r, g, b);
+ }
+ strip.show();
+}
+
+/*
+ Set Servo Value for:
+ Servo Position: index = 0
+ Position is in quarter usec.
+ Neutral ~ 6200
+ Servo Speed: index = 1
+ Speed is quarter usec / ten msec.
+ Unlimited Speed = 0
+ Servo Accel: index = 2
+ Accel is quarter usec / ten msec / eighty msec.
+ Unlimited Accel = 0
+*/
+void Statechart::setServoValue(Serial &maestro, int index, uint16_t x0, uint16_t x1, uint16_t x2) {
+ uint8_t cmd;
+ if( index == 0 ) {
+ cmd = 0x04;
+ } else if( index == 1 ) {
+ cmd = 0x07;
+ } else if( index == 2 ) {
+ cmd = 0x09;
+ } else {
+ return;
+ }
+
+ uint8_t top7;
+ uint8_t bottom7;
+
+ bottom7 = x0 & 0x7F;
+ top7 = (x0 & 0x3F80) >> 7;
+
+ maestro.putc(0xAA);
+ maestro.putc(0x0C);
+ maestro.putc(cmd);
+ maestro.putc(0x00);
+ maestro.putc(bottom7);
+ maestro.putc(top7);
+
+ bottom7 = x1 & 0x7F;
+ top7 = (x1 & 0x3F80) >> 7;
+
+ maestro.putc(0xAA);
+ maestro.putc(0x0C);
+ maestro.putc(cmd);
+ maestro.putc(0x01);
+ maestro.putc(bottom7);
+ maestro.putc(top7);
+
+ bottom7 = x2 & 0x7F;
+ top7 = (x2 & 0x3F80) >> 7;
+
+ maestro.putc(0xAA);
+ maestro.putc(0x0C);
+ maestro.putc(cmd);
+ maestro.putc(0x02);
+ maestro.putc(bottom7);
+ maestro.putc(top7);
+}
diff -r 000000000000 -r e72b30045834 statechart.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/statechart.h Mon Apr 06 20:27:47 2015 +0000
@@ -0,0 +1,38 @@
+#ifndef STATECHART_H
+#define STATECHART_H
+
+#define MINBRITE (0.2)
+#define MAXBRITE (0.5)
+#define PI (3.141592653589793)
+
+class Statechart{
+public:
+ enum actState_t {
+ REST,
+ LOOK_LEFT,
+ LOOK_RIGHT,
+ FLOP,
+ WALK,
+ FLAP,
+ BATTERY_LOW
+ };
+
+ enum emoState_t {
+ CURIOUS,
+ MAD,
+ HAPPY,
+ SAD
+ };
+
+ void fishStatechart(int netTime, int capNum, int prox, float* xyz, Serial &maestro, WS2811 &lightStrip1, WS2811 &lightStrip2, float battery);
+
+private:
+ void showRainbow(WS2811 &strip, float sat, float brite, float hueShift);
+ void showSolidColor(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b);
+ void showSolidColorBright(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b, float newBrite);
+ void showSomeRainbow(WS2811 &strip, float hueLow, float hueHigh, float sat, float brite);
+ void setMaestroSubroutine(Serial &maestro, int subNum, int parameter, bool withParameter);
+ void setServoValue(Serial &maestro, int index, uint16_t x0, uint16_t x1, uint16_t x2);
+};
+
+#endif