Changed some code for task switching
Add test for semaphore