File content as of revision 39:e735259e1d2e:

#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 DigitalOut rled(LED_RED);       // max = 0.0
static DigitalOut gled(LED_GREEN);     // max = 0.0
// LED_BLUE is on PTD1

// D0, D1 are out. D3-D7 are OK
// D4 doesn't work.
// D5 is original.
// D6 doesn't work.
// D7 doesn't work.
// D3 works for eyes.
static PwmOut servo(D3);    // PTA5
static DigitalOut eyes(D5);     // PTA2       also redLED1; max = 1.0

// 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 minFlapTime = 0.5;
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 float wingFlapTime = maxFlapTime;

// 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, int span = 1, int skip=0)
{
    int nLEDs = strip.numPixels();
    int direction, first, last;

    if (span < 0) {
        direction = -1;
        first = nLEDs-1;
        last = -1;
        span = -span;
        skip = -skip;
    } else {
        direction = 1;
        first = 0;
        last = nLEDs;
    }

    for (int i = first; i != last; i += direction)
    {
        uint8_t r, g, b;
        float hue = (i * hueRange / nLEDs) + hueShift;
        HSBtoRGB(hue, sat, brite, &r, &g, &b);
        if ((i + skip) % span == 0)
            strip.setPixelColor((unsigned)i, r, g, b);
        else
            strip.setPixelColor((unsigned)i, 0, 0, 0);
    }
    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 = 0;
        gled = 1;
    }
    else if (pos > 0.0) {
        rled = 1;
        gled = 0;
    }
    else {
        rled = gled = 1;
    }

    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; // red LED on
    wait(0.5);
    pc.printf(".");
    rled = 1; // red LED off, green LED on
    gled = 0;
    wait(0.5);
    pc.printf(".");
    gled = 1; // green LED off, eyes on
    eyes = 1;
    wait(0.5);
    pc.printf(".");
    eyes = 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: ");

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

// rainbow that wraps around entire frame
void updateStripsRainbow()
{
    static int skip = 0;

    showRainbow(lightStrip1, 1.0, maxBrite, currentSpeed, 0.5, 3, skip);
    showRainbow(lightStrip2, 1.0, maxBrite, currentSpeed + 0.5, 0.5, -3, skip);
    refreshLightStrips();
    skip++;
    skip %= 3;
}

static float currentPosition = 0.0;

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)
    {
        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();

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

    Timer elapsedTime;
    elapsedTime.start();
    float nextWingUpdate = elapsedTime.read();
    float nextStripUpdate = nextWingUpdate;
    float nextEyeUpdate = nextWingUpdate;
    currentPosition = -1.0;

    float stripUpdateTime = 0.3;
    float eyeUpdateTime = 0.3;
    bool eyesOn = true;

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

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

        float now = elapsedTime.read();

        if (now >= nextWingUpdate) {
            positionServo(currentPosition);
            currentPosition = -currentPosition;
            nextWingUpdate = now + wingFlapTime;
        }

        if (now >= nextStripUpdate) {
            updateStripsRainbow();
            nextStripUpdate = now + stripUpdateTime;
        }

        if (now >= nextEyeUpdate) {
            if (eyesOn) eyes = 1;
            else eyes = 0;
            eyesOn = !eyesOn;
            nextEyeUpdate = now + eyeUpdateTime;
        }

        wait(0.05);
    }
}