Ned Konz / Mbed 2 deprecated Multi_WS2811_test

Test program for my Multi_WS2811 library that started out as a fork of heroic/WS2811. My library uses hardware DMA on the FRDM-KL25Z to drive up to 16 strings of WS2811 or WS2812 LEDs in parallel.

Dependencies:   Multi_WS2811 mbed MMA8451Q

Fork of WS2811 by Heroic Robotics

NOTE: I have accidentally pushed changes for another fork of this program that I used in the recent Georgetown Carnival Power Tool Races. When I get some time, I will restore the test program to its original glory.

You can see my power tool racer (Nevermore's Revenge) here

/media/uploads/bikeNomad/img_0482.jpg

This tests my FRDM-KL25Z multi-string WS2811/WS2812 library. It uses the accelerometer to change the rainbow phase on two strings of LEDs as well as the touch sense to change brightness.

A video of this program in operation is here.

Here is the library that I developed to run the LEDs:

Import libraryMulti_WS2811

Library allowing up to 16 strings of 60 WS2811 or WS2812 LEDs to be driven from a single FRDM-KL25Z board. Uses hardware DMA to do a full 800 KHz rate without much CPU burden.

Last commit 13 Jun 2015 by Ned Konz

main.cpp@39:e735259e1d2e, 2015-06-15 (annotated)

Committer:
Ned Konz
Date:
Mon Jun 15 07:24:39 2015 -0700
Revision:
39:e735259e1d2e
Parent:
38:3b1ce6902a1b
Child:
40:d5c8ce80b6c4
race version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ned Konz 34:cd56c00ed910 1 #include "mbed.h"
Ned Konz 34:cd56c00ed910 2 #include "Colors.h"
Ned Konz 34:cd56c00ed910 3 #include "MMA8451Q.h"
Ned Konz 34:cd56c00ed910 4
Ned Konz 34:cd56c00ed910 5 #define MMA8451_I2C_ADDRESS (0x1d<<1)
Ned Konz 34:cd56c00ed910 6
Ned Konz 34:cd56c00ed910 7 #define INSTANTIATE_TEMPLATES 1
Ned Konz 34:cd56c00ed910 8 #include "WS2811.h"
Ned Konz 34:cd56c00ed910 9
Ned Konz 34:cd56c00ed910 10 // I/O pin usage
Ned Konz 34:cd56c00ed910 11 // PTD2 (D11) data output for strip# 1
Ned Konz 34:cd56c00ed910 12 // PTD3 (D12) data output for strip# 2
Ned Konz 34:cd56c00ed910 13 // PTA2 (D3) blinking eyes output (HI = ON)
Ned Konz 36:9ee1ec2135d5 14 // PTA5 (D5) servomotor (20 msec period; 1.0-2.0msec ON)
Ned Konz 34:cd56c00ed910 15
Ned Konz 36:9ee1ec2135d5 16 const unsigned DATA_OUT_PIN1 = 2; // PTD2
Ned Konz 36:9ee1ec2135d5 17 const unsigned DATA_OUT_PIN2 = 3; // PTD3
Ned Konz 38:3b1ce6902a1b 18
Ned Konz 38:3b1ce6902a1b 19 // actually, sides have 21 LEDs each, and ends have 10 LEDs each.
Ned Konz 38:3b1ce6902a1b 20 const unsigned MAX_LEDS_PER_STRIP = 31;
Ned Konz 34:cd56c00ed910 21
Ned Konz 34:cd56c00ed910 22 // per LED: 3 * 20 mA = 60mA max
Ned Konz 34:cd56c00ed910 23 // 60 LEDs: 60 * 60mA = 3600 mA max
Ned Konz 34:cd56c00ed910 24 // 120 LEDs: 7200 mA max
Ned Konz 36:9ee1ec2135d5 25 const unsigned nLEDs = MAX_LEDS_PER_STRIP;
Ned Konz 34:cd56c00ed910 26
Ned Konz 34:cd56c00ed910 27 template class WS2811<MAX_LEDS_PER_STRIP>;
Ned Konz 34:cd56c00ed910 28
Ned Konz 34:cd56c00ed910 29 typedef WS2811<MAX_LEDS_PER_STRIP> MyWS2811;
Ned Konz 34:cd56c00ed910 30
Ned Konz 38:3b1ce6902a1b 31 static MyWS2811 lightStrip1(nLEDs, DATA_OUT_PIN1);
Ned Konz 38:3b1ce6902a1b 32 static MyWS2811 lightStrip2(nLEDs, DATA_OUT_PIN2);
Ned Konz 34:cd56c00ed910 33
Ned Konz 34:cd56c00ed910 34 Serial pc(USBTX, USBRX);
Ned Konz 34:cd56c00ed910 35
Ned Konz 38:3b1ce6902a1b 36 // accelerometer
Ned Konz 38:3b1ce6902a1b 37 static MMA8451Q acc(PTE25, PTE24, MMA8451_I2C_ADDRESS);
Ned Konz 38:3b1ce6902a1b 38
Ned Konz 38:3b1ce6902a1b 39 // RGB LED on FRDM board
Ned Konz 39:e735259e1d2e 40 static DigitalOut rled(LED_RED); // max = 0.0
Ned Konz 39:e735259e1d2e 41 static DigitalOut gled(LED_GREEN); // max = 0.0
Ned Konz 34:cd56c00ed910 42 // LED_BLUE is on PTD1
Ned Konz 38:3b1ce6902a1b 43
Ned Konz 39:e735259e1d2e 44 // D0, D1 are out. D3-D7 are OK
Ned Konz 39:e735259e1d2e 45 // D4 doesn't work.
Ned Konz 39:e735259e1d2e 46 // D5 is original.
Ned Konz 39:e735259e1d2e 47 // D6 doesn't work.
Ned Konz 39:e735259e1d2e 48 // D7 doesn't work.
Ned Konz 39:e735259e1d2e 49 // D3 works for eyes.
Ned Konz 39:e735259e1d2e 50 static PwmOut servo(D3); // PTA5
Ned Konz 39:e735259e1d2e 51 static DigitalOut eyes(D5); // PTA2 also redLED1; max = 1.0
Ned Konz 39:e735259e1d2e 52
Ned Konz 39:e735259e1d2e 53 // static DigitalOut greenLED2(D4); // max = 1.0
Ned Konz 38:3b1ce6902a1b 54 static DigitalIn button1(D6); // low=ON, debounced
Ned Konz 39:e735259e1d2e 55 // static DigitalIn button2(D7); // low=ON, debounced
Ned Konz 34:cd56c00ed910 56
Ned Konz 36:9ee1ec2135d5 57 // Limits
Ned Konz 36:9ee1ec2135d5 58 const float maxBrite = 0.5;
Ned Konz 36:9ee1ec2135d5 59 const float minServo = -0.7; // -1.0 = -60°
Ned Konz 36:9ee1ec2135d5 60 const float maxServo = 0.6; // 1.0 = +60°
Ned Konz 39:e735259e1d2e 61
Ned Konz 39:e735259e1d2e 62 // const float minFlapTime = (maxServo - minServo) * 0.17; // 0.17 seconds / 60° at 4.8V
Ned Konz 39:e735259e1d2e 63 const float minFlapTime = 0.5;
Ned Konz 38:3b1ce6902a1b 64 const float maxFlapTime = 1.0;
Ned Konz 38:3b1ce6902a1b 65
Ned Konz 38:3b1ce6902a1b 66 // Globals
Ned Konz 38:3b1ce6902a1b 67 // test run was about 6 meters in about 5 seconds, for
Ned Konz 38:3b1ce6902a1b 68 // a final velocity of 2.4m/s, and a uniform acceleration of 0.48 m/s^2
Ned Konz 38:3b1ce6902a1b 69 // or about 0.048g
Ned Konz 38:3b1ce6902a1b 70 static float restZAccel; // in m/s^2 1g = 9.8 m/s^2
Ned Konz 38:3b1ce6902a1b 71 static float currentZAccel; // in m/s^2
Ned Konz 38:3b1ce6902a1b 72 static float currentSpeed; // in m/s
Ned Konz 38:3b1ce6902a1b 73
Ned Konz 38:3b1ce6902a1b 74 const float speedUpdateInterval = 0.1;
Ned Konz 38:3b1ce6902a1b 75 static Ticker speedUpdateTicker;
Ned Konz 38:3b1ce6902a1b 76
Ned Konz 38:3b1ce6902a1b 77 static float wingFlapTime = maxFlapTime;
Ned Konz 38:3b1ce6902a1b 78
Ned Konz 38:3b1ce6902a1b 79 // we have to know delta T to compute speed.
Ned Konz 38:3b1ce6902a1b 80 // So this is called at speedUpdateInterval seconds intervals.
Ned Konz 38:3b1ce6902a1b 81 static void updateSpeedAndAcceleration()
Ned Konz 38:3b1ce6902a1b 82 {
Ned Konz 38:3b1ce6902a1b 83 currentZAccel = acc.getAccZ() * 9.8;
Ned Konz 38:3b1ce6902a1b 84 currentSpeed += (currentZAccel - restZAccel) * speedUpdateInterval;
Ned Konz 38:3b1ce6902a1b 85 }
Ned Konz 38:3b1ce6902a1b 86
Ned Konz 38:3b1ce6902a1b 87 static void resetSpeedAndAcceleration()
Ned Konz 38:3b1ce6902a1b 88 {
Ned Konz 38:3b1ce6902a1b 89 restZAccel = currentZAccel;
Ned Konz 38:3b1ce6902a1b 90 currentSpeed = 0.0;
Ned Konz 38:3b1ce6902a1b 91 }
Ned Konz 34:cd56c00ed910 92
Ned Konz 34:cd56c00ed910 93 // @brief sets different colors in each of the LEDs of a strip
Ned Konz 34:cd56c00ed910 94 // @param strip the light strip
Ned Konz 34:cd56c00ed910 95 // @param sat saturation, 0.0 - 1.0
Ned Konz 34:cd56c00ed910 96 // @param brite brightness, 0.0 - 1.0
Ned Konz 34:cd56c00ed910 97 // @param hueShift shift, 0.0 - 1.0 is equivalent to 0 - 360 degrees
Ned Konz 39:e735259e1d2e 98 static void showRainbow(MyWS2811 &strip, float sat, float brite, float hueShift, float hueRange = 1.0, int span = 1, int skip=0)
Ned Konz 34:cd56c00ed910 99 {
Ned Konz 39:e735259e1d2e 100 int nLEDs = strip.numPixels();
Ned Konz 39:e735259e1d2e 101 int direction, first, last;
Ned Konz 39:e735259e1d2e 102
Ned Konz 39:e735259e1d2e 103 if (span < 0) {
Ned Konz 39:e735259e1d2e 104 direction = -1;
Ned Konz 39:e735259e1d2e 105 first = nLEDs-1;
Ned Konz 39:e735259e1d2e 106 last = -1;
Ned Konz 39:e735259e1d2e 107 span = -span;
Ned Konz 39:e735259e1d2e 108 skip = -skip;
Ned Konz 39:e735259e1d2e 109 } else {
Ned Konz 39:e735259e1d2e 110 direction = 1;
Ned Konz 39:e735259e1d2e 111 first = 0;
Ned Konz 39:e735259e1d2e 112 last = nLEDs;
Ned Konz 39:e735259e1d2e 113 }
Ned Konz 39:e735259e1d2e 114
Ned Konz 39:e735259e1d2e 115 for (int i = first; i != last; i += direction)
Ned Konz 38:3b1ce6902a1b 116 {
Ned Konz 34:cd56c00ed910 117 uint8_t r, g, b;
Ned Konz 38:3b1ce6902a1b 118 float hue = (i * hueRange / nLEDs) + hueShift;
Ned Konz 34:cd56c00ed910 119 HSBtoRGB(hue, sat, brite, &r, &g, &b);
Ned Konz 39:e735259e1d2e 120 if ((i + skip) % span == 0)
Ned Konz 39:e735259e1d2e 121 strip.setPixelColor((unsigned)i, r, g, b);
Ned Konz 39:e735259e1d2e 122 else
Ned Konz 39:e735259e1d2e 123 strip.setPixelColor((unsigned)i, 0, 0, 0);
Ned Konz 34:cd56c00ed910 124 }
Ned Konz 34:cd56c00ed910 125 strip.show();
Ned Konz 34:cd56c00ed910 126 }
Ned Konz 34:cd56c00ed910 127
Ned Konz 34:cd56c00ed910 128 static void showSolidColor(MyWS2811 &strip, uint8_t r, uint8_t g, uint8_t b)
Ned Konz 34:cd56c00ed910 129 {
Ned Konz 34:cd56c00ed910 130 unsigned nLEDs = strip.numPixels();
Ned Konz 38:3b1ce6902a1b 131 for (unsigned i = 0; i < nLEDs; i++)
Ned Konz 38:3b1ce6902a1b 132 {
Ned Konz 34:cd56c00ed910 133 strip.setPixelColor(i, r, g, b);
Ned Konz 34:cd56c00ed910 134 }
Ned Konz 34:cd56c00ed910 135 strip.show();
Ned Konz 34:cd56c00ed910 136 }
Ned Konz 34:cd56c00ed910 137
Ned Konz 36:9ee1ec2135d5 138 // range is -1.0 (full CCW) to +1.0 (full CW)
Ned Konz 36:9ee1ec2135d5 139 static void positionServo(float pos)
Ned Konz 36:9ee1ec2135d5 140 {
Ned Konz 38:3b1ce6902a1b 141 if (pos < minServo)
Ned Konz 38:3b1ce6902a1b 142 pos = minServo;
Ned Konz 38:3b1ce6902a1b 143 else if (pos > maxServo)
Ned Konz 38:3b1ce6902a1b 144 pos = maxServo;
Ned Konz 38:3b1ce6902a1b 145
Ned Konz 38:3b1ce6902a1b 146 if (pos < 0.0) {
Ned Konz 39:e735259e1d2e 147 rled = 0;
Ned Konz 39:e735259e1d2e 148 gled = 1;
Ned Konz 38:3b1ce6902a1b 149 }
Ned Konz 38:3b1ce6902a1b 150 else if (pos > 0.0) {
Ned Konz 39:e735259e1d2e 151 rled = 1;
Ned Konz 39:e735259e1d2e 152 gled = 0;
Ned Konz 38:3b1ce6902a1b 153 }
Ned Konz 38:3b1ce6902a1b 154 else {
Ned Konz 39:e735259e1d2e 155 rled = gled = 1;
Ned Konz 38:3b1ce6902a1b 156 }
Ned Konz 36:9ee1ec2135d5 157
Ned Konz 36:9ee1ec2135d5 158 servo.pulsewidth_us((1.5 + (pos / 2.0)) * 1000.0);
Ned Konz 36:9ee1ec2135d5 159 }
Ned Konz 36:9ee1ec2135d5 160
Ned Konz 36:9ee1ec2135d5 161 static void selfTestServo()
Ned Konz 34:cd56c00ed910 162 {
Ned Konz 36:9ee1ec2135d5 163 pc.printf("Servo:\r\n");
Ned Konz 36:9ee1ec2135d5 164 pc.printf("CCW, ");
Ned Konz 36:9ee1ec2135d5 165 positionServo(-1.0);
Ned Konz 36:9ee1ec2135d5 166 wait(1.0);
Ned Konz 36:9ee1ec2135d5 167 pc.printf("CW, ");
Ned Konz 36:9ee1ec2135d5 168 positionServo(+1.0);
Ned Konz 36:9ee1ec2135d5 169 wait(1.0);
Ned Konz 36:9ee1ec2135d5 170 pc.printf("center.\r\n");
Ned Konz 36:9ee1ec2135d5 171 positionServo(0.0);
Ned Konz 36:9ee1ec2135d5 172 }
Ned Konz 34:cd56c00ed910 173
Ned Konz 36:9ee1ec2135d5 174 static void selfTestLEDs()
Ned Konz 36:9ee1ec2135d5 175 {
Ned Konz 34:cd56c00ed910 176 pc.printf("LEDs .");
Ned Konz 39:e735259e1d2e 177 rled = 0; // red LED on
Ned Konz 38:3b1ce6902a1b 178 wait(0.5);
Ned Konz 34:cd56c00ed910 179 pc.printf(".");
Ned Konz 39:e735259e1d2e 180 rled = 1; // red LED off, green LED on
Ned Konz 39:e735259e1d2e 181 gled = 0;
Ned Konz 38:3b1ce6902a1b 182 wait(0.5);
Ned Konz 34:cd56c00ed910 183 pc.printf(".");
Ned Konz 39:e735259e1d2e 184 gled = 1; // green LED off, eyes on
Ned Konz 39:e735259e1d2e 185 eyes = 1;
Ned Konz 38:3b1ce6902a1b 186 wait(0.5);
Ned Konz 34:cd56c00ed910 187 pc.printf(".");
Ned Konz 39:e735259e1d2e 188 eyes = 0;
Ned Konz 36:9ee1ec2135d5 189 pc.printf("\r\n");
Ned Konz 36:9ee1ec2135d5 190 }
Ned Konz 34:cd56c00ed910 191
Ned Konz 38:3b1ce6902a1b 192 static void refreshLightStrips()
Ned Konz 38:3b1ce6902a1b 193 {
Ned Konz 38:3b1ce6902a1b 194 MyWS2811::startDMA();
Ned Konz 38:3b1ce6902a1b 195 // 24 bits per LED, 800kHz (1.25usec/bit)
Ned Konz 38:3b1ce6902a1b 196 // wait_us((MAX_LEDS_PER_STRIP * 24 * 10 / 8) + 100);
Ned Konz 38:3b1ce6902a1b 197 }
Ned Konz 38:3b1ce6902a1b 198
Ned Konz 38:3b1ce6902a1b 199 static void blankLightStrips()
Ned Konz 38:3b1ce6902a1b 200 {
Ned Konz 38:3b1ce6902a1b 201 showSolidColor(lightStrip1, 0, 0, 0);
Ned Konz 38:3b1ce6902a1b 202 showSolidColor(lightStrip2, 0, 0, 0);
Ned Konz 38:3b1ce6902a1b 203 refreshLightStrips();
Ned Konz 38:3b1ce6902a1b 204 }
Ned Konz 38:3b1ce6902a1b 205
Ned Konz 36:9ee1ec2135d5 206 static void selfTestLightStrips()
Ned Konz 36:9ee1ec2135d5 207 {
Ned Konz 38:3b1ce6902a1b 208 blankLightStrips();
Ned Konz 34:cd56c00ed910 209 pc.printf("light strips");
Ned Konz 37:e25d212ee3fe 210 uint8_t rgb[4] = { (uint8_t)(255 * maxBrite), 0, 0, 0 };
Ned Konz 38:3b1ce6902a1b 211 for (int i = 0; i < 3; i++)
Ned Konz 38:3b1ce6902a1b 212 {
Ned Konz 34:cd56c00ed910 213 showSolidColor(lightStrip1, rgb[0], rgb[1], rgb[2]);
Ned Konz 34:cd56c00ed910 214 showSolidColor(lightStrip2, rgb[1], rgb[2], rgb[0]);
Ned Konz 38:3b1ce6902a1b 215 refreshLightStrips();
Ned Konz 34:cd56c00ed910 216 wait(1.0);
Ned Konz 34:cd56c00ed910 217 rgb[3] = rgb[2];
Ned Konz 34:cd56c00ed910 218 rgb[2] = rgb[1];
Ned Konz 34:cd56c00ed910 219 rgb[1] = rgb[0];
Ned Konz 34:cd56c00ed910 220 rgb[0] = rgb[3];
Ned Konz 34:cd56c00ed910 221 pc.printf(".");
Ned Konz 34:cd56c00ed910 222 }
Ned Konz 38:3b1ce6902a1b 223 blankLightStrips();
Ned Konz 34:cd56c00ed910 224 pc.printf("\r\n");
Ned Konz 34:cd56c00ed910 225 }
Ned Konz 34:cd56c00ed910 226
Ned Konz 36:9ee1ec2135d5 227 static void selfTest()
Ned Konz 36:9ee1ec2135d5 228 {
Ned Konz 36:9ee1ec2135d5 229 pc.printf("self test: ");
Ned Konz 36:9ee1ec2135d5 230
Ned Konz 39:e735259e1d2e 231 selfTestLightStrips();
Ned Konz 36:9ee1ec2135d5 232 selfTestServo();
Ned Konz 36:9ee1ec2135d5 233 selfTestLEDs();
Ned Konz 38:3b1ce6902a1b 234 }
Ned Konz 38:3b1ce6902a1b 235
Ned Konz 38:3b1ce6902a1b 236 // rainbow that wraps around entire frame
Ned Konz 38:3b1ce6902a1b 237 void updateStripsRainbow()
Ned Konz 38:3b1ce6902a1b 238 {
Ned Konz 39:e735259e1d2e 239 static int skip = 0;
Ned Konz 39:e735259e1d2e 240
Ned Konz 39:e735259e1d2e 241 showRainbow(lightStrip1, 1.0, maxBrite, currentSpeed, 0.5, 3, skip);
Ned Konz 39:e735259e1d2e 242 showRainbow(lightStrip2, 1.0, maxBrite, currentSpeed + 0.5, 0.5, -3, skip);
Ned Konz 38:3b1ce6902a1b 243 refreshLightStrips();
Ned Konz 39:e735259e1d2e 244 skip++;
Ned Konz 39:e735259e1d2e 245 skip %= 3;
Ned Konz 38:3b1ce6902a1b 246 }
Ned Konz 38:3b1ce6902a1b 247
Ned Konz 39:e735259e1d2e 248 static float currentPosition = 0.0;
Ned Konz 38:3b1ce6902a1b 249
Ned Konz 38:3b1ce6902a1b 250 void setWingFlapTime(float desired)
Ned Konz 38:3b1ce6902a1b 251 {
Ned Konz 38:3b1ce6902a1b 252 static float lastWingFlapTime = 0.0;
Ned Konz 38:3b1ce6902a1b 253 if (desired < minFlapTime)
Ned Konz 38:3b1ce6902a1b 254 desired = minFlapTime;
Ned Konz 38:3b1ce6902a1b 255 else if (desired > maxFlapTime)
Ned Konz 38:3b1ce6902a1b 256 desired = maxFlapTime;
Ned Konz 38:3b1ce6902a1b 257 wingFlapTime = desired;
Ned Konz 38:3b1ce6902a1b 258
Ned Konz 38:3b1ce6902a1b 259 if (lastWingFlapTime != wingFlapTime)
Ned Konz 38:3b1ce6902a1b 260 {
Ned Konz 38:3b1ce6902a1b 261 lastWingFlapTime = wingFlapTime;
Ned Konz 38:3b1ce6902a1b 262 }
Ned Konz 38:3b1ce6902a1b 263 }
Ned Konz 38:3b1ce6902a1b 264
Ned Konz 34:cd56c00ed910 265 int main(void)
Ned Konz 34:cd56c00ed910 266 {
Ned Konz 34:cd56c00ed910 267 pc.baud(115200);
Ned Konz 36:9ee1ec2135d5 268 pc.printf("\r\n\r\nNevermore's Revenge!\r\ncompiled " __DATE__ ", " __TIME__ "\r\n");
Ned Konz 34:cd56c00ed910 269
Ned Konz 34:cd56c00ed910 270 lightStrip1.begin();
Ned Konz 34:cd56c00ed910 271 lightStrip2.begin();
Ned Konz 34:cd56c00ed910 272
Ned Konz 38:3b1ce6902a1b 273 rled = 1.0;
Ned Konz 38:3b1ce6902a1b 274 gled = 1.0;
Ned Konz 39:e735259e1d2e 275 // greenLED2 = 0.0;
Ned Konz 36:9ee1ec2135d5 276 servo.period_ms(20);
Ned Konz 34:cd56c00ed910 277
Ned Konz 34:cd56c00ed910 278 selfTest();
Ned Konz 34:cd56c00ed910 279
Ned Konz 38:3b1ce6902a1b 280 resetSpeedAndAcceleration();
Ned Konz 38:3b1ce6902a1b 281 speedUpdateTicker.attach(updateSpeedAndAcceleration, speedUpdateInterval);
Ned Konz 38:3b1ce6902a1b 282
Ned Konz 39:e735259e1d2e 283 Timer elapsedTime;
Ned Konz 39:e735259e1d2e 284 elapsedTime.start();
Ned Konz 39:e735259e1d2e 285 float nextWingUpdate = elapsedTime.read();
Ned Konz 39:e735259e1d2e 286 float nextStripUpdate = nextWingUpdate;
Ned Konz 39:e735259e1d2e 287 float nextEyeUpdate = nextWingUpdate;
Ned Konz 39:e735259e1d2e 288 currentPosition = -1.0;
Ned Konz 39:e735259e1d2e 289
Ned Konz 39:e735259e1d2e 290 float stripUpdateTime = 0.3;
Ned Konz 39:e735259e1d2e 291 float eyeUpdateTime = 0.3;
Ned Konz 39:e735259e1d2e 292 bool eyesOn = true;
Ned Konz 34:cd56c00ed910 293
Ned Konz 38:3b1ce6902a1b 294 float lastCurrentSpeed = 0.0;
Ned Konz 38:3b1ce6902a1b 295 for (;; )
Ned Konz 38:3b1ce6902a1b 296 {
Ned Konz 38:3b1ce6902a1b 297 float relativeAccel = fabs(currentZAccel - restZAccel);
Ned Konz 39:e735259e1d2e 298 if ((relativeAccel < 1.0) || !button1.read())
Ned Konz 38:3b1ce6902a1b 299 {
Ned Konz 38:3b1ce6902a1b 300 resetSpeedAndAcceleration();
Ned Konz 38:3b1ce6902a1b 301 setWingFlapTime(maxFlapTime);
Ned Konz 39:e735259e1d2e 302 stripUpdateTime = 0.3;
Ned Konz 38:3b1ce6902a1b 303 }
Ned Konz 38:3b1ce6902a1b 304 else
Ned Konz 38:3b1ce6902a1b 305 {
Ned Konz 38:3b1ce6902a1b 306 setWingFlapTime(minFlapTime);
Ned Konz 39:e735259e1d2e 307 stripUpdateTime = 0.1;
Ned Konz 38:3b1ce6902a1b 308 }
Ned Konz 38:3b1ce6902a1b 309
Ned Konz 38:3b1ce6902a1b 310 if (lastCurrentSpeed != currentSpeed)
Ned Konz 38:3b1ce6902a1b 311 {
Ned Konz 38:3b1ce6902a1b 312 lastCurrentSpeed = currentSpeed;
Ned Konz 38:3b1ce6902a1b 313 pc.printf("%f %f %f\r\n", relativeAccel, currentSpeed, wingFlapTime);
Ned Konz 38:3b1ce6902a1b 314 }
Ned Konz 39:e735259e1d2e 315
Ned Konz 39:e735259e1d2e 316 float now = elapsedTime.read();
Ned Konz 39:e735259e1d2e 317
Ned Konz 39:e735259e1d2e 318 if (now >= nextWingUpdate) {
Ned Konz 39:e735259e1d2e 319 positionServo(currentPosition);
Ned Konz 39:e735259e1d2e 320 currentPosition = -currentPosition;
Ned Konz 39:e735259e1d2e 321 nextWingUpdate = now + wingFlapTime;
Ned Konz 39:e735259e1d2e 322 }
Ned Konz 39:e735259e1d2e 323
Ned Konz 39:e735259e1d2e 324 if (now >= nextStripUpdate) {
Ned Konz 39:e735259e1d2e 325 updateStripsRainbow();
Ned Konz 39:e735259e1d2e 326 nextStripUpdate = now + stripUpdateTime;
Ned Konz 39:e735259e1d2e 327 }
Ned Konz 39:e735259e1d2e 328
Ned Konz 39:e735259e1d2e 329 if (now >= nextEyeUpdate) {
Ned Konz 39:e735259e1d2e 330 if (eyesOn) eyes = 1;
Ned Konz 39:e735259e1d2e 331 else eyes = 0;
Ned Konz 39:e735259e1d2e 332 eyesOn = !eyesOn;
Ned Konz 39:e735259e1d2e 333 nextEyeUpdate = now + eyeUpdateTime;
Ned Konz 39:e735259e1d2e 334 }
Ned Konz 39:e735259e1d2e 335
Ned Konz 39:e735259e1d2e 336 wait(0.05);
Ned Konz 34:cd56c00ed910 337 }
Ned Konz 34:cd56c00ed910 338 }