diff --git a/pwm/servo/CMakeLists.txt b/pwm/servo/CMakeLists.txt
new file mode 100755
index 000000000..a8bb7951c
--- /dev/null
+++ b/pwm/servo/CMakeLists.txt
@@ -0,0 +1,5 @@
+add_executable(servo main.c servo.c)
+
+target_link_libraries(servo pico_stdlib hardware_pwm hardware_clocks)
+
+pico_add_extra_outputs(servo)
\ No newline at end of file
diff --git a/pwm/servo/main.c b/pwm/servo/main.c
new file mode 100755
index 000000000..f2bd7441d
--- /dev/null
+++ b/pwm/servo/main.c
@@ -0,0 +1,16 @@
+#include "pico/stdlib.h"
+#include "servo.h"
+#include <stdbool.h>
+
+int main()
+{
+    servo_init(1, 50);
+    for(;;)
+    {
+        servo_put(1, 0, true);
+        sleep_ms(1000);
+
+        servo_put(1, 90, true);
+        sleep_ms(1000);
+    }
+}
\ No newline at end of file
diff --git a/pwm/servo/servo.c b/pwm/servo/servo.c
new file mode 100755
index 000000000..2b1b516a0
--- /dev/null
+++ b/pwm/servo/servo.c
@@ -0,0 +1,56 @@
+#include "servo.h"
+#include "pico/stdlib.h"
+#include "hardware/pwm.h"
+#include "hardware/clocks.h"
+#include <stdbool.h>
+
+#define DUTY_MIN 2400
+#define DUTY_MAX 8000
+#define TOP_MAX 65534
+
+long map(long x, long in_min, long in_max, long out_min, long out_max) {
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+void servo_put(int gpio, int angle, bool wait_write)
+{
+    uint slice = pwm_gpio_to_slice_num(gpio);
+    uint channel = pwm_gpio_to_channel(gpio);
+    uint32_t top = pwm_hw->slice[slice].top;
+    int duty_u16 = map(angle, 0, 180, DUTY_MIN, DUTY_MAX);
+    uint32_t cc = duty_u16 * (top + 1) / 65535;
+    pwm_set_chan_level(slice, channel, cc);
+    pwm_set_enabled(slice, true);
+
+    if(wait_write)
+        sleep_ms(1000);
+}
+
+void servo_init(int gpio, int base_frequency)
+{
+    gpio_set_function(gpio, GPIO_FUNC_PWM);
+    uint slice = pwm_gpio_to_slice_num(gpio);
+    uint channel = pwm_gpio_to_channel(gpio);
+
+
+    uint32_t source_hz = clock_get_hz(clk_sys);
+        uint freq = 50;
+        uint32_t div16_top = 16 * source_hz / freq;
+        uint32_t top = 1;
+        for (;;) {
+            if (div16_top >= 16 * 5 && div16_top % 5 == 0 && top * 5 <= TOP_MAX) {
+                div16_top /= 5;
+                top *= 5;
+            } else if (div16_top >= 16 * 3 && div16_top % 3 == 0 && top * 3 <= TOP_MAX) {
+                div16_top /= 3;
+                top *= 3;
+            } else if (div16_top >= 16 * 2 && top * 2 <= TOP_MAX) {
+                div16_top /= 2;
+                top *= 2;
+            } else {
+                break;
+            }
+        }
+        pwm_hw->slice[slice].div = div16_top;
+        pwm_hw->slice[slice].top = top;
+}
\ No newline at end of file
diff --git a/pwm/servo/servo.h b/pwm/servo/servo.h
new file mode 100755
index 000000000..85cb83ad8
--- /dev/null
+++ b/pwm/servo/servo.h
@@ -0,0 +1,24 @@
+#include <stdbool.h>
+/**
+ * @brief initialize PWM pins and frequency clock
+ * 
+ * @param gpio PWM Pin
+ * @param base_frequency set Servo Frequency - 50Hz is common
+ */
+void servo_init(int gpio, int base_frequency);
+
+
+/**
+ * @brief Map a range of values to another range
+ * @return long 
+ */
+long map(long x, long in_min, long in_max, long out_min, long out_max);
+
+/**
+ * @brief Write an angle to the Servo
+ * 
+ * @param gpio PWM pin attached to the servo
+ * @param angle Angle from 1-180
+ * @param wait_write Wait for write to finish
+ */
+void servo_put(int gpio, int angle, bool wait_write);
\ No newline at end of file