diff --git a/RidhaOs/Inc/ridhaOs.h b/RidhaOs/Inc/ridhaOs.h
index c6809c6..7f007ca 100644
--- a/RidhaOs/Inc/ridhaOs.h
+++ b/RidhaOs/Inc/ridhaOs.h
@@ -3,6 +3,7 @@
 
 #include "RIDHAOS_CONF.h"
 #include "ridhaOsScheduler.h"
+#include "ridhaOsSemaphore.h"
 /* This Struct used to restore next thread and save the running thread */
 typedef struct tControlBlock
 {
diff --git a/RidhaOs/Inc/ridhaOsSemaphore.h b/RidhaOs/Inc/ridhaOsSemaphore.h
new file mode 100644
index 0000000..a69888e
--- /dev/null
+++ b/RidhaOs/Inc/ridhaOsSemaphore.h
@@ -0,0 +1,11 @@
+#ifndef __R_SEMAPHORE_H__
+#define __R_SEMAPHORE_H__
+
+#include "ridhaOs.h"
+
+void ridhaOsSemaphoreInit(int32_t *semaphore, int32_t value);
+void ridhaOsSemaphoreSet(int32_t * semaphore);
+void ridhaOsSemaphoreWait(int32_t * semaphore);
+
+
+#endif /* __R_SEMAPHORE_H__ */
\ No newline at end of file
diff --git a/RidhaOs/Src/ridhaOsSemaphore.c b/RidhaOs/Src/ridhaOsSemaphore.c
new file mode 100644
index 0000000..0ed7f6f
--- /dev/null
+++ b/RidhaOs/Src/ridhaOsSemaphore.c
@@ -0,0 +1,25 @@
+#include "ridhaOsSemaphore.h"
+
+void ridhaOsSemaphoreInit(int32_t *semaphore, int32_t value)
+{
+    *semaphore = value;
+}
+
+void ridhaOsSemaphoreSet(int32_t * semaphore)
+{
+    __disable_irq();
+    *semaphore  += 1;
+    __enable_irq(); 
+}
+
+void ridhaOsSemaphoreWait(int32_t * semaphore)
+{
+    __disable_irq();
+    while (*semaphore <= 0)
+    {
+        __disable_irq();
+        __enable_irq();
+    }
+    *semaphore -= 1;
+    __enable_irq();
+}
\ No newline at end of file
diff --git a/main.c b/main.c
index 897c1c6..589e789 100644
--- a/main.c
+++ b/main.c
@@ -5,6 +5,8 @@
 #include "fsl_common.h"
 #include "ridhaOs.h"
 
+int32_t semaphore1, semaphore2;
+
 
 typedef uint32_t TaskProfiler;
 TaskProfiler Task0_Profiler, Task1_Profiler, Task2_Profiler;
@@ -22,12 +24,18 @@ void SystemInitHook()
 int main(void){
 
 	SystemInitHook();
-	/*Add threads */
+	uart_init();
+	
+	/* Init Semaphore */
+	ridhaOsSemaphoreInit(&semaphore1, 1);
+	ridhaOsSemaphoreInit(&semaphore2, 0);
+	
+	/* Add threads */
 	ridhaOsAddThreads(&task0, &task1, &task2);
 	ridhaOsStart();
-
-	/* We can't reach this point of program */
 	
+	/* We can't reach this point of program */
+	while(1);
 }
 
 
@@ -36,7 +44,10 @@ void task0(void)
 	Task0_Profiler = 0;
 	while(1)
 	{
-		//PRINTF("Task 0");
+		ridhaOsSemaphoreWait(&semaphore1);
+		PRINTF("Task 0\n");
+		ridhaOsSemaphoreSet(&semaphore2);
+
 		Task0_Profiler++;
 	}
 }
@@ -46,7 +57,10 @@ void task1(void)
 	Task1_Profiler = 0;
 	while(1)
 	{
-		//PRINTF("Task 1");
+		ridhaOsSemaphoreWait(&semaphore2);
+		PRINTF("Task 1\n");
+		ridhaOsSemaphoreSet(&semaphore1);
+
 		Task1_Profiler++;
 	}
 }
@@ -56,7 +70,7 @@ void task2(void)
 	Task2_Profiler = 0;
 	while(1)
 	{
-		//PRINTF("Task 2");
+		//PRINTF("Task 2\n");
 		Task2_Profiler++;
 	}
 }