diff --git a/src/include/pwm.h b/src/include/pwm.h
new file mode 100644
index 0000000..7e72af3
--- /dev/null
+++ b/src/include/pwm.h
@@ -0,0 +1,5 @@
+#define OMEGA 314.1592654
+
+void pwm_init(void);
+void pwm_worker(void);
+void test_pwm(void);
\ No newline at end of file
diff --git a/src/pwm.c b/src/pwm.c
new file mode 100644
index 0000000..495105a
--- /dev/null
+++ b/src/pwm.c
@@ -0,0 +1,45 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <math.h>
+
+#include "pwm.h"
+#include "timer.h"
+
+int interrupt_received = 0;
+uint32_t interrupt_counter = 0;
+
+void pwm_init(void)
+{
+	timer_subToT1Overflow(test_pwm);
+}
+
+void pwm_worker()
+{
+	if (!interrupt_received)
+		return;
+
+	double amp = 0.0;
+
+	/* t_n is the time of nth pulse */
+	/* factor 0.00005 because of 1/20 kHz */
+	double t_n = interrupt_counter * 0.00005;
+
+	amp = sin(2*M_PI*50*t_n);
+
+	if (amp > 0.0)
+		timer_setT1CompareMatch((uint16_t)(amp * timer_getT1Top()));
+	else
+		timer_setT1CompareMatch(0);
+
+	/* Let's wait for next interrupt */
+	interrupt_received = 0;
+}
+
+void test_pwm(void)
+{
+	interrupt_received = 1;
+	interrupt_counter++;
+
+	if (interrupt_counter == 400)
+		interrupt_counter = 0;
+}