55 lines
1.7 KiB
C++
55 lines
1.7 KiB
C++
|
#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;
|
||
|
|
}
|
||
|
|
}
|