Added more tests

test/encoder_test.cpp

@@ -0,0 +1,104 @@
+#include "encoder_test.hpp"
+#include "defines.h"
+#include "driver/gpio.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "freertos/queue.h"
+#include "soc/gpio_struct.h"
+
+typedef struct {
+  uint8_t encoder_id; // 0 = top, 1 = bottom
+  int8_t  direction;  // +1 or -1
+} enc_event_t;
+
+typedef struct {
+  gpio_num_t pin_a;
+  gpio_num_t pin_b;
+  uint8_t    id;
+  uint8_t    last_a;
+  uint8_t    last_b;
+  int8_t     base;
+} enc_state_t;
+
+static QueueHandle_t enc_queue;
+static enc_state_t   enc_states[2];
+static bool          led_state = false;
+
+static void IRAM_ATTR enc_isr(void* arg) {
+  enc_state_t* s = (enc_state_t*)arg;
+
+  uint32_t levels = GPIO.in.val;
+  uint8_t a = (levels >> s->pin_a) & 1;
+  uint8_t b = (levels >> s->pin_b) & 1;
+
+  if (a != s->last_a) {
+    if (!a) s->base += b ? 1 : -1;
+    else    s->base += b ? -1 : 1;
+  } else if (b != s->last_b) {
+    if (!b) s->base += a ? -1 : 1;
+    else    s->base += a ? 1 : -1;
+  }
+
+  s->last_a = a;
+  s->last_b = b;
+
+  enc_event_t evt;
+  evt.encoder_id = s->id;
+  BaseType_t woken = pdFALSE;
+
+  if (s->base >= 4) {
+    s->base -= 4;
+    evt.direction = 1;
+    led_state = !led_state;
+    gpio_set_level(debugLED, led_state);
+    xQueueSendFromISR(enc_queue, &evt, &woken);
+    portYIELD_FROM_ISR(woken);
+  } else if (s->base <= -4) {
+    s->base += 4;
+    evt.direction = -1;
+    led_state = !led_state;
+    gpio_set_level(debugLED, led_state);
+    xQueueSendFromISR(enc_queue, &evt, &woken);
+    portYIELD_FROM_ISR(woken);
+  }
+}
+
+void encoder_test() {
+  // LED
+  gpio_reset_pin(debugLED);
+  gpio_set_direction(debugLED, GPIO_MODE_OUTPUT);
+  gpio_set_level(debugLED, 0);
+
+  enc_queue = xQueueCreate(32, sizeof(enc_event_t));
+
+  // Init encoder states
+  enc_states[0] = { ENCODER_PIN_A, ENCODER_PIN_B, 0, 0, 0, 0 };
+  enc_states[1] = { InputEnc_PIN_A, InputEnc_PIN_B, 1, 0, 0, 0 };
+
+  // Configure encoder pins
+  gpio_config_t io_conf = {};
+  io_conf.intr_type    = GPIO_INTR_ANYEDGE;
+  io_conf.mode         = GPIO_MODE_INPUT;
+  io_conf.pull_up_en   = GPIO_PULLUP_ENABLE;
+  io_conf.pull_down_en = GPIO_PULLDOWN_DISABLE;
+  io_conf.pin_bit_mask = (1ULL << ENCODER_PIN_A) | (1ULL << ENCODER_PIN_B)
+                       | (1ULL << InputEnc_PIN_A) | (1ULL << InputEnc_PIN_B);
+  gpio_config(&io_conf);
+
+  gpio_install_isr_service(ESP_INTR_FLAG_LEVEL1);
+  gpio_isr_handler_add(ENCODER_PIN_A,  enc_isr, &enc_states[0]);
+  gpio_isr_handler_add(ENCODER_PIN_B,  enc_isr, &enc_states[0]);
+  gpio_isr_handler_add(InputEnc_PIN_A, enc_isr, &enc_states[1]);
+  gpio_isr_handler_add(InputEnc_PIN_B, enc_isr, &enc_states[1]);
+
+  printf("Encoder test running...\n");
+
+  enc_event_t evt;
+  while (true) {
+    if (xQueueReceive(enc_queue, &evt, portMAX_DELAY) == pdTRUE) {
+      printf("[%s] %s\n",
+        evt.encoder_id == 0 ? "TOP   " : "BOTTOM",
+        evt.direction  == 1 ? "CW  (+1)" : "CCW (-1)");
+    }
+  }
+}

test/encoder_test.hpp

@@ -0,0 +1,6 @@
+#ifndef ENCODER_TEST_H
+#define ENCODER_TEST_H
+
+void encoder_test();
+
+#endif

test/servo_test.cpp

@@ -0,0 +1,54 @@
+#include "servo_test.hpp"
+#include "defines.h"
+#include "driver/gpio.h"
+#include "driver/ledc.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+
+void servo_test() {
+  // LEDC timer
+  ledc_timer_config_t ledc_timer = {};
+  ledc_timer.speed_mode      = LEDC_LOW_SPEED_MODE;
+  ledc_timer.timer_num       = LEDC_TIMER_0;
+  ledc_timer.duty_resolution = LEDC_TIMER_16_BIT;
+  ledc_timer.freq_hz         = 50;
+  ledc_timer.clk_cfg         = LEDC_USE_RC_FAST_CLK;
+  ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
+
+  // LEDC channel
+  ledc_channel_config_t ledc_channel = {};
+  ledc_channel.speed_mode = LEDC_LOW_SPEED_MODE;
+  ledc_channel.channel    = servoLEDCChannel;
+  ledc_channel.timer_sel  = LEDC_TIMER_0;
+  ledc_channel.intr_type  = LEDC_INTR_DISABLE;
+  ledc_channel.gpio_num   = servoPin;
+  ledc_channel.duty       = offSpeed;
+  ledc_channel.hpoint     = 0;
+  ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
+
+  // Servo switch
+  gpio_reset_pin(servoSwitch);
+  gpio_set_direction(servoSwitch, GPIO_MODE_OUTPUT);
+  gpio_set_level(servoSwitch, 0);
+
+  // Debug LED
+  gpio_reset_pin(debugLED);
+  gpio_set_direction(debugLED, GPIO_MODE_OUTPUT);
+  gpio_set_level(debugLED, 0);
+
+  // Cycle: CW on -> off -> CCW on -> off -> ...
+  static const uint32_t duties[4] = { cwSpeed, offSpeed, ccwSpeed, offSpeed };
+  static const bool     switches[4] = { true, false, true, false };
+
+  int step = 0;
+  while (true) {
+    vTaskDelay(pdMS_TO_TICKS(5000));
+
+    ledc_set_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel, duties[step]);
+    ledc_update_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel);
+    gpio_set_level(servoSwitch, switches[step] ? 1 : 0);
+    gpio_set_level(debugLED,    switches[step] ? 1 : 0);
+
+    step = (step + 1) % 4;
+  }
+}

test/servo_test.hpp

@@ -0,0 +1,6 @@
+#ifndef SERVO_TEST_H
+#define SERVO_TEST_H
+
+void servo_test();
+
+#endif