Updated Drivers

drivers/0x05_servo/0x05_servo.c

@@ -47,24 +47,43 @@
 #define STEP_DEGREES   10
 #define STEP_DELAY_MS  150
 
+
+/**
+ * @brief Sweep the servo between start and end angles in given steps
+ *
+ * Iterates from start to end with the given step increment, setting
+ * each angle and printing the current position with a delay between steps.
+ *
+ * @param start Starting angle in degrees
+ * @param end   Ending angle in degrees
+ * @param step  Increment per iteration (negative for descending)
+ */
+static void _sweep_angle(int start, int end, int step) {
+    for (int angle = start; (step > 0) ? angle <= end : angle >= end; angle += step) {
+        servo_set_angle((float)angle);
+        printf("Angle: %3d deg\r\n", angle);
+        sleep_ms(STEP_DELAY_MS);
+    }
+}
+
+
+/**
+ * @brief Application entry point for the servo sweep demo
+ *
+ * Initializes the servo on GPIO and continuously sweeps 0-180-0 degrees
+ * in STEP_DEGREES increments, reporting each angle over UART.
+ *
+ * @return int Does not return
+ */
 int main(void) {
     stdio_init_all();
     servo_init(SERVO_GPIO);
-
     printf("Servo driver initialized on GPIO %d\r\n", SERVO_GPIO);
-    printf("Sweeping 0 -> 180 -> 0 degrees in %d-degree steps\r\n",
-           STEP_DEGREES);
-
+    printf("Sweeping 0 -> 180 -> 0 degrees in %d-degree steps\r\n", STEP_DEGREES);
     while (true) {
-        for (int angle = 0; angle <= 180; angle += STEP_DEGREES) {
-            servo_set_angle((float)angle);
-            printf("Angle: %3d deg\r\n", angle);
-            sleep_ms(STEP_DELAY_MS);
-        }
-        for (int angle = 180; angle >= 0; angle -= STEP_DEGREES) {
-            servo_set_angle((float)angle);
-            printf("Angle: %3d deg\r\n", angle);
-            sleep_ms(STEP_DELAY_MS);
-        }
+        _sweep_angle(0, 180, STEP_DEGREES);
+        _sweep_angle(180, 0, -STEP_DEGREES);
+    }
+}
     }
 }

drivers/0x05_servo/servo.c

@@ -32,18 +32,17 @@
 #include "hardware/pwm.h"
 #include "hardware/clocks.h"
 
-// Default servo pulse range (SG90 typical)
 static const uint16_t SERVO_DEFAULT_MIN_US = 1000;
 static const uint16_t SERVO_DEFAULT_MAX_US = 2000;
 
-// internal state
 static uint8_t servo_pin = 0;
 static uint servo_slice = 0;
 static uint servo_chan = 0;
-static uint32_t servo_wrap = 20000 - 1; // wrap to map microseconds for 50Hz
+static uint32_t servo_wrap = 20000 - 1;
 static float servo_hz = 50.0f;
 static bool servo_initialized = false;
 
+
 /**
  * @brief Convert a pulse width in microseconds to a PWM counter level
  *
@@ -53,45 +52,52 @@ static bool servo_initialized = false;
  * @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; // 20000us
+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);
 }
 
-void servo_init(uint8_t pin) {
-    servo_pin = pin;
-
-    // Configure GPIO for PWM
-    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);
 
+/**
+ * @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();
-
-    // Calculate clock divider to achieve 50 Hz with our chosen wrap
     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; // not initialized
-    // clamp to defaults
+    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);
+    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;
-    // linear map 0..180 -> min_us..max_us
     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);