Diff: main.cpp

Revision:

38:3b1ce6902a1b

Parent:

37:e25d212ee3fe

Child:

39:e735259e1d2e

--- a/main.cpp	Fri Jun 12 20:13:03 2015 -0700
+++ b/main.cpp	Sat Jun 13 00:18:26 2015 -0700
@@ -8,8 +8,6 @@
 #include "WS2811.h"
 
 // I/O pin usage
-// PTD0 TPM0 CH0 monitor
-// PTD1 TPM0 CH1 monitor
 // PTD2 (D11) data output for strip# 1
 // PTD3 (D12) data output for strip# 2
 // PTA2 (D3) blinking eyes output (HI = ON)
@@ -17,7 +15,9 @@
 
 const unsigned DATA_OUT_PIN1 = 2; // PTD2
 const unsigned DATA_OUT_PIN2 = 3; // PTD3
-const unsigned MAX_LEDS_PER_STRIP = 32;
+
+// 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
@@ -28,38 +28,76 @@
 
 typedef WS2811<MAX_LEDS_PER_STRIP> MyWS2811;
 
-MyWS2811 lightStrip1(nLEDs, DATA_OUT_PIN1);
-MyWS2811 lightStrip2(nLEDs, DATA_OUT_PIN2);
+static MyWS2811 lightStrip1(nLEDs, DATA_OUT_PIN1);
+static MyWS2811 lightStrip2(nLEDs, DATA_OUT_PIN2);
 
 Serial pc(USBTX, USBRX);
 
-MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
-PwmOut rled(LED_RED);       // max = 0.0
-PwmOut gled(LED_GREEN);     // max = 0.0
+// 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
-PwmOut eyes(D3);            // also redLED1; max = 1.0
-PwmOut servo(D5);
-DigitalOut greenLED2(D4);   // max = 1.0
-DigitalIn button1(D6);      // low=ON, debounced
-DigitalIn button2(D7);      // low=ON, debounced
+
+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)
+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++) {
+    for (unsigned i = 0; i < nLEDs; i++)
+    {
         uint8_t r, g, b;
-        float hue = ((float)i / (float)nLEDs) + hueShift;
+        float hue = (i * hueRange / nLEDs) + hueShift;
         HSBtoRGB(hue, sat, brite, &r, &g, &b);
         strip.setPixelColor(i, r, g, b);
     }
@@ -69,7 +107,8 @@
 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++) {
+    for (unsigned i = 0; i < nLEDs; i++)
+    {
         strip.setPixelColor(i, r, g, b);
     }
     strip.show();
@@ -78,8 +117,22 @@
 // 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 < 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);
 }
@@ -101,29 +154,44 @@
 {
     pc.printf("LEDs .");
     rled = 0.0; // red LED on
-    wait(1.0);
+    wait(0.5);
     pc.printf(".");
     rled = 1.0; // red LED off, green LED on
     gled = 0.0;
-    wait(1.0);
+    wait(0.5);
     pc.printf(".");
     gled = 1.0; // green LED off, eyes on
     eyes = 1.0;
-    wait(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++) {
+    for (int i = 0; i < 3; i++)
+    {
         showSolidColor(lightStrip1, rgb[0], rgb[1], rgb[2]);
         showSolidColor(lightStrip2, rgb[1], rgb[2], rgb[0]);
-        MyWS2811::startDMA();
-        if (i == 2) break;
+        refreshLightStrips();
         wait(1.0);
         rgb[3] = rgb[2];
         rgb[2] = rgb[1];
@@ -131,7 +199,7 @@
         rgb[0] = rgb[3];
         pc.printf(".");
     }
-
+    blankLightStrips();
     pc.printf("\r\n");
 }
 
@@ -144,6 +212,60 @@
     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);
@@ -152,29 +274,40 @@
     lightStrip1.begin();
     lightStrip2.begin();
 
-    rled = 1.0;
-    gled = 1.0;
-    eyes = 0.0;
+    rled      = 1.0;
+    gled      = 1.0;
     greenLED2 = 0.0;
     servo.period_ms(20);
 
     selfTest();
 
-    float xyz[3];
-    unsigned rep = 0;
+    eyeUpdateTicker.attach(updateEyes, 0.05);
+    wingUpdateTicker.attach(updateWings, wingFlapTime);
+
+    resetSpeedAndAcceleration();
+    speedUpdateTicker.attach(updateSpeedAndAcceleration, speedUpdateInterval);
+
+    stripUpdateTicker.attach(updateStripsRainbow, 0.1);
 
-    for (;;) {
-        rep ++;
-        acc.getAccAllAxis(xyz);
-        pc.printf("%8u x: %f y: %f z: %f\r\n", rep, xyz[0], xyz[1], xyz[2]);
-        rled = 1.0 - fabs(xyz[0]);
-        gled = 1.0 - fabs(xyz[1]);
-        eyes = fabs(xyz[2]);
-        showRainbow(lightStrip1, 1.0, maxBrite, fabs(xyz[0]));
-        showRainbow(lightStrip2, 1.0, maxBrite, fabs(xyz[1]));
-        MyWS2811::startDMA();
-        MyWS2811::wait_for_dma_done();
-        wait_us(100);
+    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);
     }
 }
-