mirror of
https://github.com/mytechnotalent/Embedded-Hacking.git
synced 2026-05-22 07:37:00 +02:00
Kevin Thomas
c46bcba401
Public function docstrings belong exclusively in .h headers per coding convention. Only static helper functions and static variables retain docstrings in .c files. Removed 413 duplicate docstrings across 185 files (15 CBM + 15 C SDK projects). All 30 projects rebuild with zero errors.
107 lines
4.0 KiB
C
107 lines
4.0 KiB
C
/**
|
|
* @file servo.c
|
|
* @brief Implementation of a simple SG90 servo driver using PWM (50Hz)
|
|
* @author Kevin Thomas
|
|
* @date 2025
|
|
*
|
|
* MIT License
|
|
*
|
|
* Copyright (c) 2025 Kevin Thomas
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
* of this software and associated documentation files (the "Software"), to deal
|
|
* in the Software without restriction, including without limitation the rights
|
|
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
* copies of the Software, and to permit persons to whom the Software is
|
|
* furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in all
|
|
* copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
* SOFTWARE.
|
|
*/
|
|
|
|
#include "servo.h"
|
|
#include "pico/stdlib.h"
|
|
#include "hardware/pwm.h"
|
|
#include "hardware/clocks.h"
|
|
|
|
/** @brief Default minimum pulse width in microseconds */
|
|
static const uint16_t SERVO_DEFAULT_MIN_US = 1000;
|
|
/** @brief Default maximum pulse width in microseconds */
|
|
static const uint16_t SERVO_DEFAULT_MAX_US = 2000;
|
|
|
|
/** @brief GPIO pin assigned to the servo */
|
|
static uint8_t servo_pin = 0;
|
|
/** @brief PWM hardware slice for the servo pin */
|
|
static uint servo_slice = 0;
|
|
/** @brief PWM channel within the servo slice */
|
|
static uint servo_chan = 0;
|
|
/** @brief PWM counter wrap value for 50 Hz servo */
|
|
static uint32_t servo_wrap = 20000 - 1;
|
|
/** @brief Servo PWM frequency in Hz */
|
|
static float servo_hz = 50.0f;
|
|
/** @brief Flag indicating servo has been initialized */
|
|
static bool servo_initialized = false;
|
|
|
|
/**
|
|
* @brief Convert a pulse width in microseconds to a PWM counter level
|
|
*
|
|
* Uses the configured PWM wrap and servo frequency to map pulse time
|
|
* into the channel compare value expected by the PWM hardware.
|
|
*
|
|
* @param pulse_us Pulse width in microseconds
|
|
* @return uint32_t PWM level suitable for pwm_set_chan_level()
|
|
*/
|
|
static uint32_t _pulse_us_to_level(uint32_t pulse_us) {
|
|
const float period_us = 1000000.0f / servo_hz;
|
|
float counts_per_us = (servo_wrap + 1) / period_us;
|
|
return (uint32_t)(pulse_us * counts_per_us + 0.5f);
|
|
}
|
|
|
|
/**
|
|
* @brief Build and apply the PWM slice configuration for 50 Hz servo
|
|
*
|
|
* Computes the clock divider from the system clock to achieve the
|
|
* target servo frequency with the chosen wrap value, then starts
|
|
* the PWM slice.
|
|
*/
|
|
static void _apply_servo_config(void) {
|
|
pwm_config config = pwm_get_default_config();
|
|
const uint32_t sys_clock_hz = clock_get_hz(clk_sys);
|
|
float clock_div = (float)sys_clock_hz / (servo_hz * (servo_wrap + 1));
|
|
pwm_config_set_clkdiv(&config, clock_div);
|
|
pwm_config_set_wrap(&config, servo_wrap);
|
|
pwm_init(servo_slice, &config, true);
|
|
}
|
|
|
|
void servo_init(uint8_t pin) {
|
|
servo_pin = pin;
|
|
gpio_set_function(servo_pin, GPIO_FUNC_PWM);
|
|
servo_slice = pwm_gpio_to_slice_num(servo_pin);
|
|
servo_chan = pwm_gpio_to_channel(servo_pin);
|
|
_apply_servo_config();
|
|
servo_initialized = true;
|
|
}
|
|
|
|
void servo_set_pulse_us(uint16_t pulse_us) {
|
|
if (!servo_initialized) return;
|
|
if (pulse_us < SERVO_DEFAULT_MIN_US) pulse_us = SERVO_DEFAULT_MIN_US;
|
|
if (pulse_us > SERVO_DEFAULT_MAX_US) pulse_us = SERVO_DEFAULT_MAX_US;
|
|
uint32_t level = _pulse_us_to_level(pulse_us);
|
|
pwm_set_chan_level(servo_slice, servo_chan, level);
|
|
}
|
|
|
|
void servo_set_angle(float degrees) {
|
|
if (degrees < 0.0f) degrees = 0.0f;
|
|
if (degrees > 180.0f) degrees = 180.0f;
|
|
float ratio = degrees / 180.0f;
|
|
uint16_t pulse = (uint16_t)(SERVO_DEFAULT_MIN_US + ratio * (SERVO_DEFAULT_MAX_US - SERVO_DEFAULT_MIN_US) + 0.5f);
|
|
servo_set_pulse_us(pulse);
|
|
} |