![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
StateChart for behavior of FlippityFlappity in presentation. Behavior here is predictable and deterministic.
Dependencies: MMA8451Q Multi_WS2811_Demo TSI mbed
Revision 0:8f759cbfc3c9, committed 2015-04-06
- Comitter:
- ryanfeng
- Date:
- Mon Apr 06 20:29:33 2015 +0000
- Commit message:
- Demo Code for presentation
Changed in this revision
diff -r 000000000000 -r 8f759cbfc3c9 MMA8451Q.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MMA8451Q.lib Mon Apr 06 20:29:33 2015 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/emilmont/code/MMA8451Q/#c4d879a39775
diff -r 000000000000 -r 8f759cbfc3c9 Multi_WS2811.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Multi_WS2811.lib Mon Apr 06 20:29:33 2015 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/ryanfeng/code/Multi_WS2811_Demo/#c4123ca9b02c
diff -r 000000000000 -r 8f759cbfc3c9 TSI.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/TSI.lib Mon Apr 06 20:29:33 2015 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/TSI/#1a60ef257879
diff -r 000000000000 -r 8f759cbfc3c9 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Apr 06 20:29:33 2015 +0000 @@ -0,0 +1,107 @@ +#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] = { 0x04, 0x04, 0x04, // Tail-side Body + 0x04, 0x04, 0x04, // Tail-side End + 0x07, 0x07, 0x07, // Head-side Body + 0x05, 0x05, 0x05,}; // Head-side End + + unsigned char sweetRelease[12] = { 0x03, 0x03, 0x03, // Tail-side Body + 0x03, 0x03, 0x03, // 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); + } +} + + \ No newline at end of file
diff -r 000000000000 -r 8f759cbfc3c9 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Apr 06 20:29:33 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 8f759cbfc3c9 mpr121.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mpr121.cpp Mon Apr 06 20:29:33 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 8f759cbfc3c9 mpr121.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mpr121.h Mon Apr 06 20:29:33 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 8f759cbfc3c9 statechart.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/statechart.cpp Mon Apr 06 20:29:33 2015 +0000 @@ -0,0 +1,705 @@ +#include <mbed.h> + +#include "WS2811.h" +#include "Colors.h" + +#include <cmath> +#include <statechart.h> + +/* TODO LIST + [!] FSM for behavior + [!] Simplify FSM for practicality + [!] Write State Transitions + [!] Validate Transitions + [!] Validate Speeds, Accels + [!] Write State Actions + [!] Test LOOK_LEFT action + [!] Test LOOK_RIGHT action + [!] Test FLOP action + [ ] Test WALK action + [!] Test FLAP action +*/ + +void Statechart::fishStatechart(int netTime, int capNum, int prox, float* xyz, Serial& maestro, WS2811& lightStrip1, WS2811& lightStrip2, float battery) { + static actState_t actState = REST; + static emoState_t emoState = HAPPY; + static int timeAtManeuverStart = 0; + + /* + // IMPORTANT: NEVER RUN THE LIPO BATTERIES BELOW 80% OF THEIR MAX CAPACITY + if( actState != BATTERY_LOW && battery < 0.57 ) { + actState = BATTERY_LOW; + timeAtManeuverStart = netTime; + }*/ + + bool cap[12] = {false,false,false, + false,false,false, + false,false,false, + false,false,false}; + int i; + int temp = capNum; + bool touched = false; + for (i=0; i<12; i++) { + if (((temp>>i)&0x01)==1) { + cap[i] = true; + touched = true; + } + } + + bool trans = (( rand() % 100 ) == 0); + int randNum; + uint16_t p0, p1, p2; + + // state transitions + switch(actState) { + case REST: + switch(emoState) { + case CURIOUS: + //if touched REST & HAPPY + if( touched ) { + emoState = HAPPY; + } + //LOOK_LEFT, LOOK_RIGHT + else if( trans ) { + randNum = rand() % 2; + if( randNum == 0 ) { + actState = LOOK_LEFT; + timeAtManeuverStart = netTime; + } else if( randNum == 1 ) { + actState = LOOK_RIGHT; + timeAtManeuverStart = netTime; + } + } + break; + case MAD: + //if touched back LOOK_LEFT + if( cap[6] || cap[9] ) { + actState = LOOK_LEFT; + timeAtManeuverStart = netTime; + } + //if touched front LOOK_RIGHT + else if( cap[7] || cap[10] ) { + actState = LOOK_RIGHT; + timeAtManeuverStart = netTime; + } + //FLAP + else if( cap[0] || cap[1] || cap[2] || cap[3] || cap[4] || cap[5] ) { + actState = FLAP; + timeAtManeuverStart = netTime; + } + break; + case HAPPY: + //if touched FLAP + if( cap[6] || cap[7] || cap[9] || cap[10] ) { + actState = FLAP; + timeAtManeuverStart = netTime; + } + //REST & MAD + else if( cap[0] || cap[1] || cap[2] || cap[3] || cap[4] || cap[5] ) { + emoState = MAD; + timeAtManeuverStart = netTime; + } + break; + case SAD: + //FLOP + if( cap[6] || cap[7] || cap[9] || cap[10] ) { + actState = FLOP; + } else if( cap[0] || cap[1] || cap[2] || cap[3] || cap[4] || cap[5] ) { + actState = FLAP; + timeAtManeuverStart = netTime; + } + break; + } //switch(emoState) { + break; //case REST: + case LOOK_LEFT: + switch(emoState) { + case CURIOUS: + //REST + if( netTime - timeAtManeuverStart > 3000 ) { + actState = REST; + timeAtManeuverStart = netTime; + } + break; + case MAD: //HEY + //REST + if( netTime - timeAtManeuverStart > 3000 ) { + actState = REST; + timeAtManeuverStart = netTime; + } + break; + case HAPPY: + // + break; + case SAD: + // + break; + } //switch(emoState) { + break; //case LOOK_LEFT: + case LOOK_RIGHT: + switch(emoState) { + case CURIOUS: + //REST + if( netTime - timeAtManeuverStart > 3000 ) { + actState = REST; + timeAtManeuverStart = netTime; + } + break; + case MAD: //HEY + //REST + if( netTime - timeAtManeuverStart > 3000 ) { + actState = REST; + timeAtManeuverStart = netTime; + } + break; + case HAPPY: + // + break; + case SAD: + // + break; + } //switch(emoState) { + break; //case LOOK_RIGHT: + case FLOP: + switch(emoState) { + case CURIOUS: + // + break; + case MAD: + // + break; + case HAPPY: + // + break; + case SAD: + //REST + if( netTime - timeAtManeuverStart > 3000 ) { + actState = REST; + timeAtManeuverStart = netTime; + } + break; + } //switch(emoState) { + break; //case FLOP: + case WALK: + switch(emoState) { + case CURIOUS: + // + break; + case MAD: + // + break; + case HAPPY: + // + break; + case SAD: + // + break; + } //switch(emoState) { + break; //case WALK: + case FLAP: + switch(emoState) { + case CURIOUS: + // + break; + case MAD: //Tantrum + //if touched REST & HAPPY + if( touched ) { + actState = REST; + emoState = SAD; + } + break; + case HAPPY: + //REST + if( netTime - timeAtManeuverStart > 3000 ) { + actState = REST; + timeAtManeuverStart = netTime; + } + break; + case SAD: //Sleep + //if touched REST & HAPPY + if( touched ) { + actState = REST; + emoState = CURIOUS; + timeAtManeuverStart = netTime; + } + break; + } //switch(emoState) { + break; //case FLAP: + case BATTERY_LOW: + break; + } //switch(actState) { + + // define what states do + switch(actState){ + case REST: + switch( emoState ) { + case CURIOUS: + showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000))); + showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000))); + setServoValue(maestro, 1, 50, 50, 50); + setServoValue(maestro, 2, 0, 0, 0); + p0 = 6200; + p1 = 6200; + p2 = 6200; + break; + case MAD: + if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) { + showSolidColorBright(lightStrip1, 255, 0, 0, 0.5); + showSolidColorBright(lightStrip2, 255, 0, 0, 0.5); + } else { + showSolidColorBright(lightStrip1, 255, 0, 0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000))); + showSolidColorBright(lightStrip2, 255, 0, 0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000))); + } + setServoValue(maestro, 1, 60, 60, 60); + setServoValue(maestro, 2, 30, 30, 30); + p0 = 6200; + p1 = 6200; + p2 = 6200; + break; + case HAPPY: + showRainbow(lightStrip1, 1.0, MINBRITE + (MAXBRITE - MINBRITE) * abs(xyz[0]), 0.5 + 0.5 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)); + showRainbow(lightStrip2, 1.0, MINBRITE + (MAXBRITE - MINBRITE) * abs(xyz[1]), 0.5 + 0.5 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000)); + setServoValue(maestro, 1, 100, 100, 100); + setServoValue(maestro, 2, 0, 0, 0); + p0 = 6200; + p1 = 6200; + p2 = 6200; + break; + case SAD: + if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) { + showSolidColorBright(lightStrip1, 0, 0, 255, 0.2); + showSolidColorBright(lightStrip2, 0, 0, 255, 0.2); + } else { + showSolidColorBright(lightStrip1, 0, 0, 255, (float)(0.35 - 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000))); + showSolidColorBright(lightStrip2, 0, 0, 255, (float)(0.35 - 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/2000))); + } + setServoValue(maestro, 1, 20, 20, 20); + setServoValue(maestro, 2, 0, 0, 0); + p0 = 6200; + p1 = 6200; + p2 = 6200; + break; + } //switch(emoState) { + WS2811::startDMA(); + setServoValue(maestro, 0, p0, p1, p2); + break; //case REST: + case LOOK_LEFT: + switch( emoState ) { + case CURIOUS: //Search >.> + showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.4 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000))); + showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.3 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000))); + setServoValue(maestro, 1, 50, 50, 50); + setServoValue(maestro, 2, 0, 0, 0); + if( netTime - timeAtManeuverStart > 3000 ) { + p0 = 6000; + p1 = 6000; + p2 = 6000; + } else { + p0 = 3000; + p1 = 8000; + p2 = 6200; + } + break; + case MAD: //HEY >:( + showSolidColorBright(lightStrip1, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000)))); + showSolidColorBright(lightStrip2, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000)))); + setServoValue(maestro, 1, 100, 100, 100); + setServoValue(maestro, 2, 0, 0, 0); + if( netTime - timeAtManeuverStart > 3000 ) { + p0 = 6000; + p1 = 6000; + p2 = 6000; + } else { + p0 = 3000; + p1 = 8000; + p2 = 6200; + } + break; + case HAPPY: //Nudge '.^ + showSolidColorBright(lightStrip1, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500))); + showSolidColorBright(lightStrip2, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500))); + setServoValue(maestro, 1, 70, 70, 70); + setServoValue(maestro, 2, 0, 0, 0); + if( netTime - timeAtManeuverStart > 3000 ) { + p0 = 6000; + p1 = 6000; + p2 = 6000; + } else { + p0 = 3000; + p1 = 8000; + p2 = 6200; + } + break; + case SAD: //Unreachable state O.O + showSolidColor(lightStrip1, 0, 0, 0); + showSolidColor(lightStrip2, 0, 0, 0); + setServoValue(maestro, 1, 10, 10, 10); + setServoValue(maestro, 2, 10, 10, 10); + p0 = 6000; + p1 = 6000; + p2 = 6000; + break; + } //switch(emoState) { + WS2811::startDMA(); + setServoValue(maestro, 0, p0, p1, p2); + break; //case LOOK_LEFT: + case LOOK_RIGHT: + switch( emoState ) { + case CURIOUS: //Search <.< + showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.4 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000))); + showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.3 + 0.1 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1000))); + setServoValue(maestro, 1, 50, 50, 50); + setServoValue(maestro, 2, 0, 0, 0); + if( netTime - timeAtManeuverStart > 3000 ) { + p0 = 6000; + p1 = 6000; + p2 = 6000; + } else { + p0 = 5000; + p1 = 10000; + p2 = 6200; + } + break; + case MAD: //HEY >:( + showSolidColorBright(lightStrip1, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000)))); + showSolidColorBright(lightStrip2, 128, 0, 0, 0.5 - abs((float)(0.3 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/1000)))); + setServoValue(maestro, 1, 120, 120, 120); + setServoValue(maestro, 2, 0, 0, 0); + if( netTime - timeAtManeuverStart > 3000 ) { + p0 = 6000; + p1 = 6000; + p2 = 6000; + } else { + p0 = 5000; + p1 = 10000; + p2 = 6200; + } + break; + case HAPPY: //Nudge '.^ + showSolidColorBright(lightStrip1, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500))); + showSolidColorBright(lightStrip2, 190, 53, 232, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/1500))); + setServoValue(maestro, 1, 70, 70, 70); + setServoValue(maestro, 2, 0, 0, 0); + if( netTime - timeAtManeuverStart > 3000 ) { + p0 = 6000; + p1 = 6000; + p2 = 6000; + } else { + p0 = 5000; + p1 = 10000; + p2 = 6200; + } + break; + case SAD: //Unreachable state O.O + showSolidColor(lightStrip1, 255, 255, 255); + showSolidColor(lightStrip2, 255, 255, 255); + setServoValue(maestro, 1, 10, 10, 10); + setServoValue(maestro, 2, 10, 10, 10); + p0 = 6000; + p1 = 6000; + p2 = 6000; + break; + } //switch(emoState) { + WS2811::startDMA(); + setServoValue(maestro, 0, p0, p1, p2); + break; //case LOOK_RIGHT + case FLOP: + switch( emoState ) { + case CURIOUS: //Unreachable state O.O + showSolidColor(lightStrip1, 0, 0, 0); + showSolidColor(lightStrip2, 0, 0, 0); + setServoValue(maestro, 1, 50, 50, 50); + setServoValue(maestro, 2, 0, 0, 0); + p0 = 6000; + p1 = 6000; + p2 = 6000; + break; + case MAD: //Unreachable state O.O + showSolidColor(lightStrip1, 0, 0, 0); + showSolidColor(lightStrip2, 0, 0, 0); + setServoValue(maestro, 1, 50, 50, 50); + setServoValue(maestro, 2, 50, 50, 50); + p0 = 6000; + p1 = 6000; + p2 = 6000; + break; + case HAPPY: //Hey! :D + if( (netTime - timeAtManeuverStart) % 4000 < 2000 ) { + showRainbow(lightStrip1, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/500)), abs(xyz[0])); + showRainbow(lightStrip2, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/500)), abs(xyz[1])); + } else { + showRainbow(lightStrip1, 1.0, 0.2, abs(xyz[0])); + showRainbow(lightStrip2, 1.0, 0.2, abs(xyz[1])); + } + setServoValue(maestro, 1, 80, 80, 80); + setServoValue(maestro, 2, 0, 0, 0); + p0 = 4000; + if( (netTime - timeAtManeuverStart) % 4000 > 2000 ) { + p1 = 6200; + p2 = 6200; + } else { + p1 = 2500; + p2 = 2500; + } + break; + case SAD: //Sigh... :/ + showSolidColorBright(lightStrip1, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000))); + showSolidColorBright(lightStrip2, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000))); + setServoValue(maestro, 1, 20, 20, 20); + setServoValue(maestro, 2, 10, 10, 10); + p0 = 4000; + if( (netTime - timeAtManeuverStart) % 4000 > 1000 ) { + p1 = 6200; + p2 = 6200; + } else { + p1 = 2500; + p2 = 2500; + } + break; + } //switch(emoState) { + WS2811::startDMA(); + setServoValue(maestro, 0, p0, p1, p2); + break; //case FLOP: + case WALK: + switch( emoState ) { + case CURIOUS: //Search o.o + if( (netTime - timeAtManeuverStart) % 3000 > 2000 ) { + showSomeRainbow(lightStrip1, 0.45 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 0.82 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000))); + showSomeRainbow(lightStrip2, 0.45 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 0.82 + 0.05 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000), 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000))); + } else { + showSomeRainbow(lightStrip1, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000))); + showSomeRainbow(lightStrip2, 0.5, 0.77, 1.0, (float)(0.35 + 0.15 * cos(2*PI*(double)(netTime - timeAtManeuverStart)/3000))); + } + setServoValue(maestro, 1, 70, 70, 70); + setServoValue(maestro, 2, 0, 0, 0); + if( (netTime - timeAtManeuverStart) % 1000 < 100 ) { + p0 = 4500; + p1 = 2000; + p2 = 3500; + } else if( (netTime - timeAtManeuverStart) % 1000 < 200 ) { + p0 = 6200; + p1 = 3500; + p2 = 2500; + } else if( (netTime - timeAtManeuverStart) % 1000 < 300 ) { + p0 = 10000; + p1 = 5500; + p2 = 3500; + } else if( (netTime - timeAtManeuverStart) % 1000 < 400 ) { + p0 = 10000; + p1 = 6500; + p2 = 4000; + } else if( (netTime - timeAtManeuverStart) % 1000 < 550 ) { + p0 = 10000; + p1 = 7000; + p2 = 5000; + } else if( (netTime - timeAtManeuverStart) % 1000 < 700 ) { + p0 = 6200; + p1 = 6200; + p2 = 6200; + } else if( (netTime - timeAtManeuverStart) % 1000 < 850 ) { + p0 = 5500; + p1 = 4500; + p2 = 4500; + } else if( (netTime - timeAtManeuverStart) % 1000 < 1000 ) { + p0 = 5000; + p1 = 4000; + p2 = 4000; + } + break; + case MAD: //Unreachable state O.O + showSolidColor(lightStrip1, 0, 0, 0); + showSolidColor(lightStrip2, 0, 0, 0); + setServoValue(maestro, 1, 50, 50, 50); + setServoValue(maestro, 2, 50, 50, 50); + p0 = 6000; + p1 = 6000; + p2 = 6000; + setServoValue(maestro, 0, 6000, 6000, 6000); + break; + case HAPPY: //Chase XD + showRainbow(lightStrip1, 1.0, 0.5, (float)((netTime - timeAtManeuverStart) % 500) / 500.0); + showRainbow(lightStrip2, 1.0, 0.5, (float)((netTime - timeAtManeuverStart) % 500) / 500.0); + setServoValue(maestro, 1, 100, 100, 100); + setServoValue(maestro, 2, 0, 0, 0); + p0 = (int)(7000 + 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 0*2*PI/3))); + p1 = (int)(5000 - 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 1*2*PI/3))); + p2 = (int)(7000 + 3000 * (0.5 * cos(8*PI*(double)(netTime - timeAtManeuverStart)/5000 + 2*2*PI/3))); + break; + case SAD: //Unreachable state O.O + showSolidColor(lightStrip1, 0, 0, 0); + showSolidColor(lightStrip2, 0, 0, 0); + setServoValue(maestro, 1, 10, 10, 10); + setServoValue(maestro, 2, 10, 10, 10); + p0 = 6000; + p1 = 6000; + p2 = 6000; + break; + } //switch(emoState) { + WS2811::startDMA(); + setServoValue(maestro, 0, p0, p1, p2); + break; //case WALK: + case FLAP: + switch( emoState ) { + case CURIOUS: //Unreachable state O.O + showSolidColor(lightStrip1, 0, 0, 0); + showSolidColor(lightStrip2, 0, 0, 0); + setServoValue(maestro, 1, 50, 50, 50); + setServoValue(maestro, 2, 0, 0, 0); + p0 = 6000; + p1 = 6000; + p2 = 6000; + break; + case MAD: //Tantrum >:X + showSolidColorBright(lightStrip1, 153, 0, 0, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750))); + showSolidColorBright(lightStrip2, 153, 0, 0, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750))); + setServoValue(maestro, 1, 100, 100, 100); + setServoValue(maestro, 2, 0, 0, 0); + p0 = 4000; + p1 = 6200 - 7400 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750); + p2 = 6200 - 7400 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/750); + break; + case HAPPY: //Laugh ^.^ + showRainbow(lightStrip1, 1.0, 0.35, (float)((netTime - timeAtManeuverStart) % 1000) / 1000.0); + showRainbow(lightStrip2, 1.0, 0.35, (float)((netTime - timeAtManeuverStart) % 1000) / 1000.0); + setServoValue(maestro, 1, 100, 100, 100); + setServoValue(maestro, 2, 0, 0, 0); + p0 = 4000; + p1 = 6200; + p2 = 3600 - 800 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/500); + break; + case SAD: //Sleep -_- + showSolidColorBright(lightStrip1, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000))); + showSolidColorBright(lightStrip2, 19, 6, 128, (float)(0.35 + 0.15 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000))); + setServoValue(maestro, 1, 20, 20, 20); + setServoValue(maestro, 2, 0, 0, 0); + p0 = 4000; + p1 = 6500; + p2 = 3200 - 1600 * sin(2*PI*(double)(netTime - timeAtManeuverStart)/3000); + break; + } //switch(emoState) { + WS2811::startDMA(); + setServoValue(maestro, 0, p0, p1, p2); + break; //case FLAP: + case BATTERY_LOW: + if( (netTime - timeAtManeuverStart) % 1000 > 500 ) { + showSolidColor(lightStrip1, 255, 0, 0); + showSolidColor(lightStrip2, 0, 0, 0); + } else { + showSolidColor(lightStrip1, 0, 0, 0); + showSolidColor(lightStrip2, 255, 0, 0); + } + WS2811::startDMA(); + break; + } //switch(actState) { +} //void Statechart::fishStateChart + +// @brief sets different colors in each of the LEDs of a strip +// @param strip the light strip +// @param sat saturation, 0.0 - 1.0 +// @param brite brightness, 0.0 - 1.0 +// @param hueShift shift, 0.0 - 1.0 is equivalent to 0 - 360 degrees +void Statechart::showRainbow(WS2811 &strip, float sat, float brite, float hueShift) +{ + unsigned nLEDs = strip.numPixels(); + for (unsigned i = 0; i < nLEDs; i++) { + uint8_t r, g, b; + float hue = ((float)i / (float)nLEDs) + hueShift; + HSBtoRGB(hue, sat, brite, &r, &g, &b); + strip.setPixelColor(i, r, g, b); + } + strip.show(); +} + +void Statechart::showSolidColor(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b) +{ + unsigned nLEDs = strip.numPixels(); + for (unsigned i = 0; i < nLEDs; i++) { + strip.setPixelColor(i, r, g, b); + } + strip.show(); +} + + +//showSolidColorBright(lightStrip1, 0, 0, 255, (float)(0.35 + 0.15 * sin(2*PI * (double)currTime / 1000))); +//showSolidColorBright(lightStrip2, 0, 0, 255, (float)(0.35 + 0.15 * sin(2*PI * (double)currTime / 1000))); +//WS2811::startDMA(); +void Statechart::showSolidColorBright(WS2811 &strip, uint8_t r, uint8_t g, uint8_t b, float newBrite) +{ + unsigned nLEDs = strip.numPixels(); + for (unsigned i = 0; i < nLEDs; i++) { + float hsbVals[3]; + RGBtoHSB(r, g, b, hsbVals); + HSBtoRGB(hsbVals[0], hsbVals[1], newBrite, &r, &g, &b); + strip.setPixelColor(i, r, g, b); + } + strip.show(); +} + +void Statechart::showSomeRainbow(WS2811 &strip, float hueLow, float hueHigh, float sat, float brite) +{ + unsigned nLEDs = strip.numPixels(); + for (unsigned i = 0; i < nLEDs; i++) { + uint8_t r, g, b; + float hue = hueLow + (hueHigh - hueLow) * ((float)i / (float)nLEDs); + HSBtoRGB(hue, sat, brite, &r, &g, &b); + strip.setPixelColor(i, r, g, b); + } + strip.show(); +} + +/* + Set Servo Value for: + Servo Position: index = 0 + Position is in quarter usec. + Neutral ~ 6200 + Servo Speed: index = 1 + Speed is quarter usec / ten msec. + Unlimited Speed = 0 + Servo Accel: index = 2 + Accel is quarter usec / ten msec / eighty msec. + Unlimited Accel = 0 +*/ +void Statechart::setServoValue(Serial &maestro, int index, uint16_t x0, uint16_t x1, uint16_t x2) { + uint8_t cmd; + if( index == 0 ) { + cmd = 0x04; + } else if( index == 1 ) { + cmd = 0x07; + } else if( index == 2 ) { + cmd = 0x09; + } else { + return; + } + + uint8_t top7; + uint8_t bottom7; + + bottom7 = x0 & 0x7F; + top7 = (x0 & 0x3F80) >> 7; + + maestro.putc(0xAA); + maestro.putc(0x0C); + maestro.putc(cmd); + maestro.putc(0x00); + maestro.putc(bottom7); + maestro.putc(top7); + + bottom7 = x1 & 0x7F; + top7 = (x1 & 0x3F80) >> 7; + + maestro.putc(0xAA); + maestro.putc(0x0C); + maestro.putc(cmd); + maestro.putc(0x01); + maestro.putc(bottom7); + maestro.putc(top7); + + bottom7 = x2 & 0x7F; + top7 = (x2 & 0x3F80) >> 7; + + maestro.putc(0xAA); + maestro.putc(0x0C); + maestro.putc(cmd); + maestro.putc(0x02); + maestro.putc(bottom7); + maestro.putc(top7); +}
diff -r 000000000000 -r 8f759cbfc3c9 statechart.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/statechart.h Mon Apr 06 20:29:33 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