File content as of revision 38:3b1ce6902a1b:

#include "mbed.h"
#include "Colors.h"
#include "MMA8451Q.h"

#define MMA8451_I2C_ADDRESS (0x1d<<1)

#define INSTANTIATE_TEMPLATES 1
#include "WS2811.h"

// I/O pin usage
// PTD2 (D11) data output for strip# 1
// PTD3 (D12) data output for strip# 2
// PTA2 (D3) blinking eyes output (HI = ON)
// PTA5 (D5) servomotor (20 msec period; 1.0-2.0msec ON)

const unsigned DATA_OUT_PIN1 = 2; // PTD2
const unsigned DATA_OUT_PIN2 = 3; // PTD3

// actually, sides have 21 LEDs each, and ends have 10 LEDs each.
const unsigned MAX_LEDS_PER_STRIP = 31;

// per LED: 3 * 20 mA = 60mA max
// 60 LEDs: 60 * 60mA = 3600 mA max
// 120 LEDs: 7200 mA max
const unsigned nLEDs = MAX_LEDS_PER_STRIP;

template class WS2811<MAX_LEDS_PER_STRIP>;

typedef WS2811<MAX_LEDS_PER_STRIP> MyWS2811;

static MyWS2811 lightStrip1(nLEDs, DATA_OUT_PIN1);
static MyWS2811 lightStrip2(nLEDs, DATA_OUT_PIN2);

Serial pc(USBTX, USBRX);

// accelerometer
static MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);

// RGB LED on FRDM board
static PwmOut rled(LED_RED);       // max = 0.0
static PwmOut gled(LED_GREEN);     // max = 0.0
// LED_BLUE is on PTD1

static PwmOut eyes(D3);            // also redLED1; max = 1.0
static PwmOut servo(D5);
static DigitalOut greenLED2(D4);   // max = 1.0
static DigitalIn button1(D6);      // low=ON, debounced
static DigitalIn button2(D7);      // low=ON, debounced

// Limits
const float maxBrite = 0.5;
const float minServo = -0.7; // -1.0 = -60°
const float maxServo = 0.6; // 1.0 = +60°
const float minFlapTime = (maxServo - minServo) * 0.17; // 0.17 seconds / 60° at 4.8V
const float maxFlapTime = 1.0;

// Globals
// test run was about 6 meters in about 5 seconds, for
// a final velocity of 2.4m/s, and a uniform acceleration of 0.48 m/s^2
// or about 0.048g
static float restZAccel;    // in m/s^2 1g = 9.8 m/s^2
static float currentZAccel;  // in m/s^2 
static float currentSpeed;  // in m/s

const float speedUpdateInterval = 0.1;
static Ticker speedUpdateTicker;

static Ticker eyeUpdateTicker;

static Ticker stripUpdateTicker;

static float wingFlapTime = maxFlapTime;
static Ticker wingUpdateTicker;

// we have to know delta T to compute speed.
// So this is called at speedUpdateInterval seconds intervals.
static void updateSpeedAndAcceleration()
{
    currentZAccel = acc.getAccZ() * 9.8;
    currentSpeed += (currentZAccel - restZAccel) * speedUpdateInterval;
}

static void resetSpeedAndAcceleration()
{
    restZAccel   = currentZAccel;
    currentSpeed = 0.0;
}

// @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
static void showRainbow(MyWS2811 &strip, float sat, float brite, float hueShift, float hueRange = 1.0)
{
    unsigned nLEDs = strip.numPixels();
    for (unsigned i = 0; i < nLEDs; i++)
    {
        uint8_t r, g, b;
        float hue = (i * hueRange / nLEDs) + hueShift;
        HSBtoRGB(hue, sat, brite, &r, &g, &b);
        strip.setPixelColor(i, r, g, b);
    }
    strip.show();
}

static void showSolidColor(MyWS2811 &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();
}

// range is -1.0 (full CCW) to +1.0 (full CW)
static void positionServo(float pos)
{
    if (pos < minServo)
        pos = minServo;
    else if (pos > maxServo)
        pos = maxServo;

    if (pos < 0.0) {
        rled = pos + 1.0;
        gled = 1.0;
    }
    else if (pos > 0.0) {
        rled = 1.0;
        gled = 1.0 - pos ;
    }
    else {
        rled = gled = 0.5;
    }

    servo.pulsewidth_us((1.5 + (pos / 2.0)) * 1000.0);
}

static void selfTestServo()
{
    pc.printf("Servo:\r\n");
    pc.printf("CCW, ");
    positionServo(-1.0);
    wait(1.0);
    pc.printf("CW, ");
    positionServo(+1.0);
    wait(1.0);
    pc.printf("center.\r\n");
    positionServo(0.0);
}

static void selfTestLEDs()
{
    pc.printf("LEDs .");
    rled = 0.0; // red LED on
    wait(0.5);
    pc.printf(".");
    rled = 1.0; // red LED off, green LED on
    gled = 0.0;
    wait(0.5);
    pc.printf(".");
    gled = 1.0; // green LED off, eyes on
    eyes = 1.0;
    wait(0.5);
    pc.printf(".");
    eyes = 0.0;
    pc.printf("\r\n");
}

static void refreshLightStrips()
{
    MyWS2811::startDMA();
    // 24 bits per LED, 800kHz (1.25usec/bit)
    // wait_us((MAX_LEDS_PER_STRIP * 24 * 10 / 8) + 100);
}

static void blankLightStrips()
{
    showSolidColor(lightStrip1, 0, 0, 0);
    showSolidColor(lightStrip2, 0, 0, 0);
    refreshLightStrips();
}

static void selfTestLightStrips()
{
    blankLightStrips();
    pc.printf("light strips");
    uint8_t rgb[4] = { (uint8_t)(255 * maxBrite), 0, 0, 0 };
    for (int i = 0; i < 3; i++)
    {
        showSolidColor(lightStrip1, rgb[0], rgb[1], rgb[2]);
        showSolidColor(lightStrip2, rgb[1], rgb[2], rgb[0]);
        refreshLightStrips();
        wait(1.0);
        rgb[3] = rgb[2];
        rgb[2] = rgb[1];
        rgb[1] = rgb[0];
        rgb[0] = rgb[3];
        pc.printf(".");
    }
    blankLightStrips();
    pc.printf("\r\n");
}

static void selfTest()
{
    pc.printf("self test: ");

    selfTestServo();
    selfTestLEDs();
    selfTestLightStrips();
}

void updateEyes()
{
    static float brite     = 1.0;
    static float increment = -0.1;

    eyes   = brite;

    brite += increment;
    if (brite >= 1.0)
    {
        increment = -0.05;
        brite     = 1.0;
    }
    else if (brite <= 0.0)
    {
        increment = 0.05;
        brite     = 0.0;
    }
}

// rainbow that wraps around entire frame
void updateStripsRainbow()
{
    showRainbow(lightStrip1, 1.0, maxBrite, currentSpeed, 0.5);
    showRainbow(lightStrip2, 1.0, maxBrite, currentSpeed + 0.5, 0.5);
    refreshLightStrips();
}

// callback
void updateWings()
{
    static float currentPosition = 1.0;

    currentPosition = -currentPosition;
    positionServo(currentPosition);
}

void setWingFlapTime(float desired)
{
    static float lastWingFlapTime = 0.0;
    if (desired < minFlapTime)
        desired = minFlapTime;
    else if (desired > maxFlapTime)
        desired = maxFlapTime;
    wingFlapTime = desired;

    if (lastWingFlapTime != wingFlapTime)
    {
        wingUpdateTicker.detach();
        wingUpdateTicker.attach(updateWings, wingFlapTime);
        lastWingFlapTime = wingFlapTime;
    }
}

int main(void)
{
    pc.baud(115200);
    pc.printf("\r\n\r\nNevermore's Revenge!\r\ncompiled " __DATE__ ", " __TIME__ "\r\n");

    lightStrip1.begin();
    lightStrip2.begin();

    rled      = 1.0;
    gled      = 1.0;
    greenLED2 = 0.0;
    servo.period_ms(20);

    selfTest();

    eyeUpdateTicker.attach(updateEyes, 0.05);
    wingUpdateTicker.attach(updateWings, wingFlapTime);

    resetSpeedAndAcceleration();
    speedUpdateTicker.attach(updateSpeedAndAcceleration, speedUpdateInterval);

    stripUpdateTicker.attach(updateStripsRainbow, 0.1);

    float lastCurrentSpeed = 0.0;
    for (;; )
    {
        float relativeAccel = fabs(currentZAccel - restZAccel);
        if ((relativeAccel < 1.0) || !button2.read())
        {
            resetSpeedAndAcceleration();
            setWingFlapTime(maxFlapTime);
        }
        else
        {
            setWingFlapTime(minFlapTime);
        }

        if (lastCurrentSpeed != currentSpeed)
        {
            lastCurrentSpeed = currentSpeed;
            pc.printf("%f %f %f\r\n", relativeAccel, currentSpeed, wingFlapTime);
        }
        wait(0.1);
    }
}