light test (success).

platformio.ini

@@ -12,5 +12,5 @@
 platform = teensy
 board = teensy41
 framework = arduino
-
 upload_protocol = teensy-cli
+lib_deps = adafruit/Adafruit NeoPixel@^1.10.1

src/main.cpp

@@ -1,4 +1,5 @@
 #include <Arduino.h>
+#include <Adafruit_NeoPixel.h>
 #include "Motor.h"
 
 // Vertical motor top
@@ -33,6 +34,13 @@
 #define HORZ_CNT_INNER  35
 #define HORZ_CNT_OUTER  36
 
+// Lights
+#define LED_FRONT 41
+#define LED_COUNT_FRONT 26
+
+#define LED_BACK  14
+#define LED_COUNT_BACK 72
+
 volatile int32_t hor_pos;
 volatile int32_t vert_pos;
 
@@ -46,6 +54,15 @@ Motor vert_down(VERT_DOWN_PWM, VERT_DOWN_AIN1, VERT_DOWN_AIN2);
 Motor horz_left(HORZ_LEFT_PWM, HORZ_LEFT_AIN1, HORZ_LEFT_AIN2);
 Motor horz_right(HORZ_RIGHT_PWM, HORZ_RIGHT_AIN1, HORZ_RIGHT_AIN2);
 
+Adafruit_NeoPixel led_front(LED_COUNT_FRONT, LED_FRONT, NEO_GBRW + NEO_KHZ800);
+Adafruit_NeoPixel led_back(LED_COUNT_BACK, LED_BACK, NEO_GBRW + NEO_KHZ800);
+
+bool back;
+bool led_on;
+int led_n;
+u_int8_t brightness;
+u_int8_t color;
+
 int32_t count(int pinA, int pinB) {
   if (digitalRead(pinA)) return digitalRead(pinB) ? -1 : 1;
   else return digitalRead(pinB) ? 1 : -1;
@@ -59,6 +76,23 @@ void vert_count() {
   vert_pos += count(VERT_CNT_INNER, VERT_CNT_OUTER);
 }
 
+/*
+Generic motor control (full speed). Call every 10us for good results.
+*/
+void mot_control(Motor mot1, Motor mot2, int32_t pos, int32_t aim) {
+  if (pos < aim) {
+    mot1.run(255, false);
+    mot2.run(127, false);
+  } else if (vert_pos > vert_aim) {
+    mot2.run(255, true);
+    mot1.run(127, true);
+  } else {
+    mot1.stop(false);
+    mot2.stop(false);
+    // vert_aim = (vert_aim == 50) ? 0 : 50;
+  }
+}
+
 void setup() {
   Serial.begin(115200);
 
@@ -94,24 +128,51 @@ void setup() {
   // attachInterrupt(digitalPinToInterrupt(HORZ_CNT_OUTER), hor_count, CHANGE);
   attachInterrupt(digitalPinToInterrupt(VERT_CNT_INNER), vert_count, CHANGE);
   // attachInterrupt(digitalPinToInterrupt(VERT_CNT_OUTER), vert_count, CHANGE);
+
+  back = true;
+  led_on = true;
+  
+  brightness = 0;
+  color = 0;
+
+  led_back.begin();
+  led_back.show();
+
+  led_front.begin();
+  led_front.show();
 }
 
 void loop() {
-
-  if (vert_pos < vert_aim) {
-    vert_down.run(255, false);
-    vert_up.run(127, false);
-  } else if (vert_pos > vert_aim) {
-    vert_up.run(255, true);
-    vert_down.run(127, true);
+  // led_front.setPixelColor(led_n, 0, 0, 0, led_on ? 255 : 0);
+  uint32_t c = led_back.Color((color == 0 || color == 1 || color == 2 || color == 9) ? brightness : 0,
+                               (color == 2 || color == 3 || color == 4 || color == 8) ? brightness : 0,
+                               (color == 4 || color == 5 || color == 6 || color == 9) ? brightness : 0,
+                               (color == 6 || color == 7 || color == 0 || color == 8) ? brightness : 0);
+  if (back) {
+    led_back.fill(c);
+    led_back.show();
   } else {
-    vert_up.stop(false);
-    vert_down.stop(false);
-    vert_aim = (vert_aim == 50) ? 0 : 50;
-    delay(100);
+    led_front.fill(c);
+    led_front.show();
+  }
+  delay(10);
+
+  if (led_on && (brightness < 255)) {
+    brightness++;
+  } else if (!led_on && (brightness > 0)) {
+    brightness--;
+  } else {
+    if (!led_on) {
+      if (color < 9) color++;
+      else {
+        color = 0;
+        back = !back;
+      }
+    }
+    led_on = !led_on;
   }
 
-  Serial.printf("hor_count %i | \t vert_count %i | \t vert_aim %i \n", hor_pos, vert_pos, vert_aim);
-
-  delayMicroseconds(10);
+  if (brightness % 8 == 0) {
+    Serial.printf("hor_pos %i | \t vert_pos %i \n", hor_pos, vert_pos);
+  }
 }