From d2884423a3c6b785dd9f68547ca1f8fcd41d2136 Mon Sep 17 00:00:00 2001 From: pulipakaa24 Date: Wed, 11 Mar 2026 19:48:37 -0500 Subject: [PATCH] Added more tests --- src/main.cpp | 4 +- test/encoder_test.cpp | 104 ++++++++++++++++++++++++++++++++++++++++++ test/encoder_test.hpp | 6 +++ test/servo_test.cpp | 54 ++++++++++++++++++++++ test/servo_test.hpp | 6 +++ 5 files changed, 173 insertions(+), 1 deletion(-) create mode 100644 test/encoder_test.cpp create mode 100644 test/encoder_test.hpp create mode 100644 test/servo_test.cpp create mode 100644 test/servo_test.hpp diff --git a/src/main.cpp b/src/main.cpp index 396b5d9..157266d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -12,6 +12,8 @@ #include "mainEventLoop.hpp" #include "max17048.h" #include "bms_test.hpp" +#include "servo_test.hpp" +#include "encoder_test.hpp" // Global encoder instances Encoder* topEnc = new Encoder(ENCODER_PIN_A, ENCODER_PIN_B); @@ -56,5 +58,5 @@ void pm_init() { extern "C" void app_main() { // pm_init(); // mainApp(); - bms_test_LED(); + encoder_test(); } \ No newline at end of file diff --git a/test/encoder_test.cpp b/test/encoder_test.cpp new file mode 100644 index 0000000..20a99d4 --- /dev/null +++ b/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)"); + } + } +} diff --git a/test/encoder_test.hpp b/test/encoder_test.hpp new file mode 100644 index 0000000..bf62083 --- /dev/null +++ b/test/encoder_test.hpp @@ -0,0 +1,6 @@ +#ifndef ENCODER_TEST_H +#define ENCODER_TEST_H + +void encoder_test(); + +#endif diff --git a/test/servo_test.cpp b/test/servo_test.cpp new file mode 100644 index 0000000..b20463c --- /dev/null +++ b/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; + } +} diff --git a/test/servo_test.hpp b/test/servo_test.hpp new file mode 100644 index 0000000..ec09fd7 --- /dev/null +++ b/test/servo_test.hpp @@ -0,0 +1,6 @@ +#ifndef SERVO_TEST_H +#define SERVO_TEST_H + +void servo_test(); + +#endif