diff --git a/hal/include/HAL/Digital.hpp b/hal/include/HAL/Digital.hpp
index 002bb16f27..b167728754 100644
--- a/hal/include/HAL/Digital.hpp
+++ b/hal/include/HAL/Digital.hpp
@@ -4,7 +4,8 @@
 #else
 #include <stdint.h>
 #endif
-#include "HAL/cpp/Synchronized.hpp"
+
+#include "HAL/cpp/priority_mutex.h"
 
 enum Mode
 {
@@ -14,6 +15,8 @@ enum Mode
 	kExternalDirection = 3
 };
 
+priority_recursive_mutex& spiGetSemaphore(uint8_t port);
+
 extern "C"
 {
 	void* initializeDigitalPort(void* port_pointer, int32_t *status);
@@ -103,8 +106,6 @@ extern "C"
 	void spiSetChipSelectActiveLow(uint8_t port, int32_t *status);
 	int32_t spiGetHandle(uint8_t port);
 	void spiSetHandle(uint8_t port, int32_t handle);
-	MUTEX_ID spiGetSemaphore(uint8_t port);
-	void spiSetSemaphore(uint8_t port, MUTEX_ID semaphore);
 
 	void i2CInitialize(uint8_t port, int32_t *status);
 	int32_t i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t *dataToSend, uint8_t sendSize, uint8_t *dataReceived, uint8_t receiveSize);
diff --git a/hal/include/HAL/Task.hpp b/hal/include/HAL/Task.hpp
index eb1ad00b11..1c8c690b3f 100644
--- a/hal/include/HAL/Task.hpp
+++ b/hal/include/HAL/Task.hpp
@@ -22,13 +22,6 @@ typedef int (*FUNCPTR) (); /* ptr to function returning int */
 typedef int STATUS;
 #endif /* _STATUS_DEFINED */
 
-#ifndef OK
-#define OK		0
-#endif /* OK */
-#ifndef ERROR
-#define ERROR		(-1)
-#endif /* ERROR */
-
 #ifdef __vxworks
 #define NULL_TASK -1
 typedef int32_t TASK;
diff --git a/hal/include/HAL/cpp/Resource.hpp b/hal/include/HAL/cpp/Resource.hpp
index 41ca9139cc..b40a0f611b 100644
--- a/hal/include/HAL/cpp/Resource.hpp
+++ b/hal/include/HAL/cpp/Resource.hpp
@@ -6,12 +6,12 @@
 #pragma once
 
 #include "../Errors.hpp"
-#include "Synchronized.hpp"
 #ifdef __vxworks
 #include <vxWorks.h>
 #else
 #include <stdint.h>
 #endif
+#include "HAL/cpp/priority_mutex.h"
 
 /**
  * The Resource class is a convenient way to track allocated resources.
@@ -25,6 +25,8 @@
 class Resource
 {
 public:
+	Resource(const Resource&) = delete;
+	Resource& operator=(const Resource&) = delete;
 	virtual ~Resource();
 	static void CreateResourceObject(Resource **r, uint32_t elements);
 	uint32_t Allocate(const char *resourceDesc);
@@ -35,11 +37,9 @@ private:
 	explicit Resource(uint32_t size);
 
 	bool *m_isAllocated;
-	ReentrantSemaphore m_allocateLock;
+	priority_recursive_mutex m_allocateLock;
 	uint32_t m_size;
 
-	static ReentrantSemaphore m_createLock;
-
-	DISALLOW_COPY_AND_ASSIGN(Resource);
+	static priority_recursive_mutex m_createLock;
 };
 
diff --git a/hal/include/HAL/cpp/Semaphore.hpp b/hal/include/HAL/cpp/Semaphore.hpp
new file mode 100644
index 0000000000..6a6374b8dd
--- /dev/null
+++ b/hal/include/HAL/cpp/Semaphore.hpp
@@ -0,0 +1,30 @@
+#pragma once
+
+#include <cstdint>
+#include <condition_variable>
+
+#include "HAL/cpp/priority_mutex.h"
+
+class Semaphore {
+ public:
+  explicit Semaphore(uint32_t count = 0);
+  Semaphore(Semaphore&&);
+  Semaphore& operator=(Semaphore&&);
+
+  void give();
+  void take();
+
+  // @return true if semaphore was locked successfully. false if not.
+  bool tryTake();
+
+  static const int32_t kNoWait = 0;
+  static const int32_t kWaitForever = -1;
+
+  static const uint32_t kEmpty = 0;
+  static const uint32_t kFull = 1;
+
+ private:
+  priority_mutex m_mutex;
+  std::condition_variable_any m_condition;
+  uint32_t m_count = 0;
+};
diff --git a/hal/include/HAL/cpp/Synchronized.hpp b/hal/include/HAL/cpp/Synchronized.hpp
deleted file mode 100644
index 57fcb44386..0000000000
--- a/hal/include/HAL/cpp/Synchronized.hpp
+++ /dev/null
@@ -1,105 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
-/*----------------------------------------------------------------------------*/
-#pragma once
-
-#include "HAL/Semaphore.hpp"
-
-// A macro for making a class move-only
-#define DISALLOW_COPY_AND_ASSIGN(TypeName)       \
-  TypeName(const TypeName&) = delete;            \
-  TypeName& operator=(const TypeName&) = delete; \
-  TypeName(TypeName&&) = default;                \
-  TypeName& operator=(TypeName&&) = default
-
-#define CRITICAL_REGION(s) { Synchronized _sync(s);
-#define END_REGION }
-// TODO: is this Better?
-#define SYNCHRONIZE(s) for (Synchronized _sync(s), int _i=0;i < 1; i++)
-
-class Synchronized;
-
-/**
- * Wrap a vxWorks semaphore (SEM_ID) for easier use in C++. For a static
- * instance, the constructor runs at program load time before main() can spawn
- * any tasks. Use that to fix race conditions in setup code.
- *
- * This uses a semM semaphore which is "reentrant" in the sense that the owning
- * task can "take" the semaphore more than once. It will need to "give" the
- * semaphore the same number of times to unlock it.
- *
- * This class is safe to use in static variables because it does not depend on
- * any other C++ static constructors or destructors.
- */
-class ReentrantSemaphore
-{
-public:
-	explicit ReentrantSemaphore()
-	{
-		m_semaphore = initializeMutexRecursive();
-	}
-	~ReentrantSemaphore()
-	{
-		deleteMutex(m_semaphore);
-	}
-
-	/**
-	 * Lock the semaphore, blocking until it's available.
-	 * @return 0 for success, -1 for error. If -1, the error will be in errno.
-	 */
-	int take()
-	{
-		return takeMutex(m_semaphore);
-	}
-
-	/**
-	 * Unlock the semaphore.
-	 * @return 0 for success, -1 for error. If -1, the error will be in errno.
-	 */
-	int give()
-	{
-		return giveMutex(m_semaphore);
-	}
-
-private:
-	MUTEX_ID m_semaphore;
-
-	friend class Synchronized;DISALLOW_COPY_AND_ASSIGN(ReentrantSemaphore);
-};
-
-/**
- * Provide easy support for critical regions.
- *
- * A critical region is an area of code that is always executed under mutual exclusion. Only
- * one task can be executing this code at any time. The idea is that code that manipulates data
- * that is shared between two or more tasks has to be prevented from executing at the same time
- * otherwise a race condition is possible when both tasks try to update the data. Typically
- * semaphores are used to ensure only single task access to the data.
- *
- * Synchronized objects are a simple wrapper around semaphores to help ensure
- * that semaphores are always unlocked (semGive) after locking (semTake).
- *
- * You allocate a Synchronized as a local variable, *not* on the heap. That
- * makes it a "stack object" whose destructor runs automatically when it goes
- * out of scope. E.g.
- *
- *   { Synchronized _sync(aReentrantSemaphore); ... critical region ... }
- */
-class Synchronized
-{
-public:
-	explicit Synchronized(MUTEX_ID);
-#ifndef __vxworks
-	explicit Synchronized(SEMAPHORE_ID);
-#endif
-	explicit Synchronized(ReentrantSemaphore&);
-	virtual ~Synchronized();
-private:
-	MUTEX_ID m_mutex;
-	SEMAPHORE_ID m_semaphore;
-
-	DISALLOW_COPY_AND_ASSIGN(Synchronized);
-};
-
diff --git a/hal/include/HAL/cpp/priority_condition_variable.h b/hal/include/HAL/cpp/priority_condition_variable.h
new file mode 100644
index 0000000000..4a74d3091a
--- /dev/null
+++ b/hal/include/HAL/cpp/priority_condition_variable.h
@@ -0,0 +1,116 @@
+#pragma once
+
+/* std::condition_variable provides the native_handle() method to return its
+ * underlying pthread_cond_t*. WPILib uses this to interface with the FRC
+ * network communication library. Since WPILib uses a custom mutex class and
+ * not std::mutex, std::condition_variable_any must be used instead.
+ * std::condition_variable_any doesn't expose its internal handle, so this
+ * class provides the same interface and implementation in addition to a
+ * native_handle() method.
+ */
+
+#include <condition_variable>
+#include <memory>
+#include "priority_mutex.h"
+
+class priority_condition_variable {
+  typedef pthread_cond_t* native_handle_type;
+  typedef std::chrono::system_clock clock_t;
+
+ public:
+  priority_condition_variable() : m_mutex(std::make_shared<std::mutex>()) {}
+  ~priority_condition_variable() = default;
+
+  priority_condition_variable(const priority_condition_variable&) = delete;
+  priority_condition_variable& operator=(const priority_condition_variable&) = delete;
+
+  void notify_one() noexcept {
+    std::lock_guard<std::mutex> lock(*m_mutex);
+    m_cond.notify_one();
+  }
+
+  void notify_all() noexcept {
+    std::lock_guard<std::mutex> lock(*m_mutex);
+    m_cond.notify_all();
+  }
+
+  template<typename Lock>
+  void wait(Lock& _lock) {
+    std::shared_ptr<std::mutex> _mutex = m_mutex;
+    std::unique_lock<std::mutex> my_lock(*_mutex);
+    Unlock<Lock> unlock(_lock);
+
+    // *mutex must be unlocked before re-locking _lock so move
+    // ownership of *_mutex lock to an object with shorter lifetime.
+    std::unique_lock<std::mutex> my_lock2(std::move(my_lock));
+    m_cond.wait(my_lock2);
+  }
+
+  template<typename Lock, typename Predicate>
+  void wait(Lock& lock, Predicate p) {
+    while (!p()) { wait(lock); }
+  }
+
+  template<typename Lock, typename Clock, typename Duration>
+  std::cv_status wait_until(Lock& _lock,
+        const std::chrono::time_point<Clock, Duration>& atime) {
+    std::shared_ptr<std::mutex> _mutex = m_mutex;
+    std::unique_lock<std::mutex> my_lock(*_mutex);
+    Unlock<Lock> unlock(_lock);
+
+    // *_mutex must be unlocked before re-locking _lock so move
+    // ownership of *_mutex lock to an object with shorter lifetime.
+    std::unique_lock<std::mutex> my_lock2(std::move(my_lock));
+    return m_cond.wait_until(my_lock2, atime);
+  }
+
+  template<typename Lock, typename Clock, typename Duration, typename Predicate>
+  bool wait_until(Lock& lock,
+      const std::chrono::time_point<Clock, Duration>& atime, Predicate p) {
+    while (!p()) {
+      if (wait_until(lock, atime) == std::cv_status::timeout) {
+        return p();
+      }
+    }
+    return true;
+  }
+
+  template<typename Lock, typename Rep, typename Period>
+  std::cv_status wait_for(Lock& lock, const std::chrono::duration<Rep, Period>& rtime) {
+    return wait_until(lock, clock_t::now() + rtime);
+  }
+
+  template<typename Lock, typename Rep, typename Period, typename Predicate>
+  bool wait_for(Lock& lock, const std::chrono::duration<Rep, Period>& rtime,
+      Predicate p) {
+    return wait_until(lock, clock_t::now() + rtime, std::move(p));
+  }
+
+  native_handle_type native_handle() {
+    return m_cond.native_handle();
+  }
+
+ private:
+  std::condition_variable m_cond;
+  std::shared_ptr<std::mutex> m_mutex;
+
+  // scoped unlock - unlocks in ctor, re-locks in dtor
+  template<typename Lock>
+  struct Unlock {
+    explicit Unlock(Lock& lk) : m_lock(lk) { lk.unlock(); }
+
+    ~Unlock() noexcept(false) {
+      if (std::uncaught_exception()) {
+        try { m_lock.lock(); } catch(...) {}
+      }
+      else {
+        m_lock.lock();
+      }
+    }
+
+    Unlock(const Unlock&) = delete;
+    Unlock& operator=(const Unlock&) = delete;
+
+    Lock& m_lock;
+  };
+};
diff --git a/hal/include/HAL/cpp/priority_mutex.h b/hal/include/HAL/cpp/priority_mutex.h
new file mode 100644
index 0000000000..ae8d73cb0f
--- /dev/null
+++ b/hal/include/HAL/cpp/priority_mutex.h
@@ -0,0 +1,67 @@
+#pragma once
+
+#include <pthread.h>
+
+// Allows usage with std::unique_lock without including <mutex> separately
+#include <mutex>
+
+class priority_recursive_mutex {
+ public:
+  typedef pthread_mutex_t *native_handle_type;
+
+  constexpr priority_recursive_mutex() noexcept = default;
+  priority_recursive_mutex(const priority_recursive_mutex &) = delete;
+  priority_recursive_mutex &operator=(const priority_recursive_mutex &) = delete;
+
+  // Lock the mutex, blocking until it's available.
+  void lock();
+
+  // Unlock the mutex.
+  void unlock();
+
+  // Tries to lock the mutex.
+  bool try_lock() noexcept;
+
+  pthread_mutex_t *native_handle();
+
+ private:
+  // Do the equivalent of setting PTHREAD_PRIO_INHERIT and
+  // PTHREAD_MUTEX_RECURSIVE_NP.
+#if __WORDSIZE == 64
+  pthread_mutex_t m_mutex = {
+      {0, 0, 0, 0, 0x20 | PTHREAD_MUTEX_RECURSIVE_NP, 0, 0, {0, 0}}};
+#else
+  pthread_mutex_t m_mutex = {
+      {0, 0, 0, 0x20 | PTHREAD_MUTEX_RECURSIVE_NP, 0, {0}}};
+#endif
+};
+
+class priority_mutex {
+ public:
+  typedef pthread_mutex_t *native_handle_type;
+
+  constexpr priority_mutex() noexcept = default;
+  priority_mutex(const priority_mutex &) = delete;
+  priority_mutex &operator=(const priority_mutex &) = delete;
+
+  // Lock the mutex, blocking until it's available.
+  void lock();
+
+  // Unlock the mutex.
+  void unlock();
+
+  // Tries to lock the mutex.
+  bool try_lock() noexcept;
+
+  pthread_mutex_t *native_handle();
+
+ private:
+  // Do the equivalent of setting PTHREAD_PRIO_INHERIT.
+#if __WORDSIZE == 64
+  pthread_mutex_t m_mutex = {
+      {0, 0, 0, 0, 0x20, 0, 0, {0, 0}}};
+#else
+  pthread_mutex_t m_mutex = {
+      {0, 0, 0, 0x20, 0, {0}}};
+#endif
+};
diff --git a/hal/lib/Athena/Analog.cpp b/hal/lib/Athena/Analog.cpp
index 9461ebbd6f..e96c07754c 100644
--- a/hal/lib/Athena/Analog.cpp
+++ b/hal/lib/Athena/Analog.cpp
@@ -1,10 +1,10 @@
 
 #include "HAL/Analog.hpp"
 
+#include "HAL/cpp/priority_mutex.h"
 #include "Port.h"
 #include "HAL/HAL.hpp"
 #include "ChipObject.h"
-#include "HAL/cpp/Synchronized.hpp"
 #include "HAL/cpp/Resource.hpp"
 #include "NetworkCommunication/AICalibration.h"
 #include "NetworkCommunication/LoadOut.h"
@@ -25,7 +25,7 @@ struct AnalogPort {
 };
 
 bool analogSampleRateSet = false;
-MUTEX_ID analogRegisterWindowSemaphore = NULL;
+priority_recursive_mutex analogRegisterWindowMutex;
 tAI* analogInputSystem = NULL;
 tAO* analogOutputSystem = NULL;
 uint32_t analogNumChannelsToActivate = 0;
@@ -41,8 +41,8 @@ bool analogSystemInitialized = false;
  * Initialize the analog System.
  */
 void initializeAnalog(int32_t *status) {
+  std::unique_lock<priority_recursive_mutex> sync(analogRegisterWindowMutex);
   if (analogSystemInitialized) return;
-  analogRegisterWindowSemaphore = initializeMutexRecursive();
   analogInputSystem = tAI::create(status);
   analogOutputSystem = tAO::create(status);
   setAnalogNumChannelsToActivate(kAnalogInputPins);
@@ -265,7 +265,7 @@ int16_t getAnalogValue(void* analog_port_pointer, int32_t *status) {
   readSelect.Averaged = false;
 
   {
-    Synchronized sync(analogRegisterWindowSemaphore);
+    std::unique_lock<priority_recursive_mutex> sync(analogRegisterWindowMutex);
     analogInputSystem->writeReadSelect(readSelect, status);
     analogInputSystem->strobeLatchOutput(status);
     value = (int16_t) analogInputSystem->readOutput(status);
@@ -296,7 +296,7 @@ int32_t getAnalogAverageValue(void* analog_port_pointer, int32_t *status) {
   readSelect.Averaged = true;
 
   {
-    Synchronized sync(analogRegisterWindowSemaphore);
+    std::unique_lock<priority_recursive_mutex> sync(analogRegisterWindowMutex);
     analogInputSystem->writeReadSelect(readSelect, status);
     analogInputSystem->strobeLatchOutput(status);
     value = (int32_t) analogInputSystem->readOutput(status);
diff --git a/hal/lib/Athena/Digital.cpp b/hal/lib/Athena/Digital.cpp
index f11721614d..beb61af8a1 100644
--- a/hal/lib/Athena/Digital.cpp
+++ b/hal/lib/Athena/Digital.cpp
@@ -4,11 +4,12 @@
 #include "Port.h"
 #include "HAL/HAL.hpp"
 #include "ChipObject.h"
-#include "HAL/cpp/Synchronized.hpp"
 #include "HAL/cpp/Resource.hpp"
+#include "HAL/cpp/priority_mutex.h"
 #include "NetworkCommunication/LoadOut.h"
 #include <stdio.h>
 #include <math.h>
+#include <mutex>
 #include "i2clib/i2c-lib.h"
 #include "spilib/spi-lib.h"
 
@@ -53,11 +54,14 @@ struct DigitalPort {
 };
 
 // XXX: Set these back to static once we figure out the memory clobbering issue
-MUTEX_ID digitalDIOSemaphore = NULL;
-MUTEX_ID digitalRelaySemaphore = NULL;
-MUTEX_ID digitalPwmSemaphore = NULL;
-MUTEX_ID digitalI2COnBoardSemaphore = NULL;
-MUTEX_ID digitalI2CMXPSemaphore = NULL;
+// Create a mutex to protect changes to the digital output values
+priority_recursive_mutex digitalDIOMutex;
+// Create a mutex to protect changes to the relay values
+priority_recursive_mutex digitalRelayMutex;
+// Create a mutex to protect changes to the DO PWM config
+priority_recursive_mutex digitalPwmMutex;
+priority_recursive_mutex digitalI2COnBoardMutex;
+priority_recursive_mutex digitalI2CMXPMutex;
 
 tDIO* digitalSystem = NULL;
 tRelay* relaySystem = NULL;
@@ -78,8 +82,8 @@ int32_t m_spiCS1Handle = 0;
 int32_t m_spiCS2Handle = 0;
 int32_t m_spiCS3Handle = 0;
 int32_t m_spiMXPHandle = 0;
-MUTEX_ID spiOnboardSemaphore = NULL;
-MUTEX_ID spiMXPSemaphore = NULL;
+priority_recursive_mutex spiOnboardSemaphore;
+priority_recursive_mutex spiMXPSemaphore;
 tSPI *spiSystem;
 
 /**
@@ -88,18 +92,6 @@ tSPI *spiSystem;
 void initializeDigital(int32_t *status) {
   if (digitalSystemsInitialized) return;
 
-  // Create a semaphore to protect changes to the digital output values
-  digitalDIOSemaphore = initializeMutexRecursive();
-
-  // Create a semaphore to protect changes to the relay values
-  digitalRelaySemaphore = initializeMutexRecursive();
-
-  // Create a semaphore to protect changes to the DO PWM config
-  digitalPwmSemaphore = initializeMutexRecursive();
-
-  digitalI2COnBoardSemaphore = initializeMutexRecursive();
-  digitalI2CMXPSemaphore = initializeMutexRecursive();
-
   Resource::CreateResourceObject(&DIOChannels, tDIO::kNumSystems * kDigitalPins);
   Resource::CreateResourceObject(&DO_PWMGenerators, tDIO::kNumPWMDutyCycleAElements + tDIO::kNumPWMDutyCycleBElements);
   Resource::CreateResourceObject(&PWMChannels, tPWM::kNumSystems * kPwmPins);
@@ -292,7 +284,7 @@ void setPWMDutyCycle(void* pwmGenerator, double dutyCycle, int32_t *status) {
   float rawDutyCycle = 256.0 * dutyCycle;
   if (rawDutyCycle > 255.5) rawDutyCycle = 255.5;
   {
-    Synchronized sync(digitalPwmSemaphore);
+	std::unique_lock<priority_recursive_mutex> sync(digitalPwmMutex);
     uint8_t pwmPeriodPower = digitalSystem->readPWMPeriodPower(status);
     if (pwmPeriodPower < 4) {
 	  // The resolution of the duty cycle drops close to the highest frequencies.
@@ -326,7 +318,7 @@ void setRelayForward(void* digital_port_pointer, bool on, int32_t *status) {
   DigitalPort* port = (DigitalPort*) digital_port_pointer;
   checkRelayChannel(port);
   {
-    Synchronized sync(digitalRelaySemaphore);
+    std::unique_lock<priority_recursive_mutex> sync(digitalRelayMutex);
     uint8_t forwardRelays = relaySystem->readValue_Forward(status);
     if (on)
       forwardRelays |= 1 << port->port.pin;
@@ -345,7 +337,7 @@ void setRelayReverse(void* digital_port_pointer, bool on, int32_t *status) {
   DigitalPort* port = (DigitalPort*) digital_port_pointer;
   checkRelayChannel(port);
   {
-    Synchronized sync(digitalRelaySemaphore);
+    std::unique_lock<priority_recursive_mutex> sync(digitalRelayMutex);
     uint8_t reverseRelays = relaySystem->readValue_Reverse(status);
     if (on)
       reverseRelays |= 1 << port->port.pin;
@@ -392,7 +384,7 @@ bool allocateDIO(void* digital_port_pointer, bool input, int32_t *status) {
   }
 
   {
-    Synchronized sync(digitalDIOSemaphore);
+    std::unique_lock<priority_recursive_mutex> sync(digitalDIOMutex);
 
     tDIO::tOutputEnable outputEnable = digitalSystem->readOutputEnable(status);
 
@@ -476,7 +468,7 @@ void setDIO(void* digital_port_pointer, short value, int32_t *status) {
       value = 1;
   }
   {
-    Synchronized sync(digitalDIOSemaphore);
+    std::unique_lock<priority_recursive_mutex> sync(digitalDIOMutex);
     tDIO::tDO currentDIO = digitalSystem->readDO(status);
 
     if(port->port.pin < kNumHeaders) {
@@ -1113,8 +1105,6 @@ uint16_t getLoopTiming(int32_t *status) {
 void spiInitialize(uint8_t port, int32_t *status) {
 	if(spiSystem == NULL)
 		spiSystem = tSPI::create(status);
-	if(spiGetSemaphore(port) == NULL)
-		spiSetSemaphore(port, initializeMutexRecursive());
 	if(spiGetHandle(port) !=0 ) return;
 	switch(port){
 	case 0:
@@ -1157,7 +1147,7 @@ void spiInitialize(uint8_t port, int32_t *status) {
  */
 int32_t spiTransaction(uint8_t port, uint8_t *dataToSend, uint8_t *dataReceived, uint8_t size)
 {
-	Synchronized sync(spiGetSemaphore(port));
+	std::unique_lock<priority_recursive_mutex> sync(spiGetSemaphore(port));
 	return spilib_writeread(spiGetHandle(port), (const char*) dataToSend, (char*) dataReceived, (int32_t) size);
 }
 
@@ -1173,7 +1163,7 @@ int32_t spiTransaction(uint8_t port, uint8_t *dataToSend, uint8_t *dataReceived,
  */
 int32_t spiWrite(uint8_t port, uint8_t* dataToSend, uint8_t sendSize)
 {
-	Synchronized sync(spiGetSemaphore(port));
+	std::unique_lock<priority_recursive_mutex> sync(spiGetSemaphore(port));
 	return spilib_write(spiGetHandle(port), (const char*) dataToSend, (int32_t) sendSize);
 }
 
@@ -1191,7 +1181,7 @@ int32_t spiWrite(uint8_t port, uint8_t* dataToSend, uint8_t sendSize)
  */
 int32_t spiRead(uint8_t port, uint8_t *buffer, uint8_t count)
 {
-	Synchronized sync(spiGetSemaphore(port));
+	std::unique_lock<priority_recursive_mutex> sync(spiGetSemaphore(port));
 	return spilib_read(spiGetHandle(port), (char*) buffer, (int32_t) count);
 }
 
@@ -1201,7 +1191,7 @@ int32_t spiRead(uint8_t port, uint8_t *buffer, uint8_t count)
  * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
  */
 void spiClose(uint8_t port) {
-	Synchronized sync(spiGetSemaphore(port));
+	std::unique_lock<priority_recursive_mutex> sync(spiGetSemaphore(port));
 	spilib_close(spiGetHandle(port));
 	spiSetHandle(port, 0);
 	return;
@@ -1214,7 +1204,7 @@ void spiClose(uint8_t port) {
  * @param speed The speed in Hz (0-1MHz)
  */
 void spiSetSpeed(uint8_t port, uint32_t speed) {
-	Synchronized sync(spiGetSemaphore(port));
+	std::unique_lock<priority_recursive_mutex> sync(spiGetSemaphore(port));
 	spilib_setspeed(spiGetHandle(port), speed);
 }
 
@@ -1227,7 +1217,7 @@ void spiSetSpeed(uint8_t port, uint32_t speed) {
  * @param clk_idle_high True to set the clock to active low, False to set the clock active high
  */
 void spiSetOpts(uint8_t port, int msb_first, int sample_on_trailing, int clk_idle_high) {
-	Synchronized sync(spiGetSemaphore(port));
+	std::unique_lock<priority_recursive_mutex> sync(spiGetSemaphore(port));
 	spilib_setopts(spiGetHandle(port), msb_first, sample_on_trailing, clk_idle_high);
 }
 
@@ -1237,7 +1227,7 @@ void spiSetOpts(uint8_t port, int msb_first, int sample_on_trailing, int clk_idl
  * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
  */
 void spiSetChipSelectActiveHigh(uint8_t port, int32_t *status){
-	Synchronized sync(spiGetSemaphore(port));
+	std::unique_lock<priority_recursive_mutex> sync(spiGetSemaphore(port));
 	if(port < 4)
 	{
 		spiSystem->writeChipSelectActiveHigh_Hdr(spiSystem->readChipSelectActiveHigh_Hdr(status) | (1<<port), status);
@@ -1254,7 +1244,7 @@ void spiSetChipSelectActiveHigh(uint8_t port, int32_t *status){
  * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
  */
 void spiSetChipSelectActiveLow(uint8_t port, int32_t *status){
-	Synchronized sync(spiGetSemaphore(port));
+	std::unique_lock<priority_recursive_mutex> sync(spiGetSemaphore(port));
 	if(port < 4)
 	{
 		spiSystem->writeChipSelectActiveHigh_Hdr(spiSystem->readChipSelectActiveHigh_Hdr(status) & ~(1<<port), status);
@@ -1272,7 +1262,7 @@ void spiSetChipSelectActiveLow(uint8_t port, int32_t *status){
  * @return The stored handle for the SPI port. 0 represents no stored handle.
  */
 int32_t spiGetHandle(uint8_t port){
-	Synchronized sync(spiGetSemaphore(port));
+	std::unique_lock<priority_recursive_mutex> sync(spiGetSemaphore(port));
 	switch(port){
 	case 0:
 		return m_spiCS0Handle;
@@ -1296,7 +1286,7 @@ int32_t spiGetHandle(uint8_t port){
  * @param handle The value of the handle for the port.
  */
 void spiSetHandle(uint8_t port, int32_t handle){
-	Synchronized sync(spiGetSemaphore(port));
+	std::unique_lock<priority_recursive_mutex> sync(spiGetSemaphore(port));
 	switch(port){
 	case 0:
 		m_spiCS0Handle = handle;
@@ -1322,28 +1312,15 @@ void spiSetHandle(uint8_t port, int32_t handle){
  * Get the semaphore for a SPI port
  *
  * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
- * @return The semaphore for the SPI port. NULL represents no stored semaphore.
+ * @return The semaphore for the SPI port.
  */
-MUTEX_ID spiGetSemaphore(uint8_t port){
+priority_recursive_mutex& spiGetSemaphore(uint8_t port) {
 	if(port < 4)
 		return spiOnboardSemaphore;
 	else
 		return spiMXPSemaphore;
 }
 
-/**
- * Set the semaphore for a SPI port
- *
- * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
- * @param semaphore The semaphore for the SPI port.
- */
-void spiSetSemaphore(uint8_t port, MUTEX_ID semaphore){
-	if (port < 4)
-		spiOnboardSemaphore = semaphore;
-	else
-		spiMXPSemaphore = semaphore;
-}
-
 /*
  * Initialize the I2C port. Opens the port if necessary and saves the handle.
  * If opening the MXP port, also sets up the pin functions appropriately
@@ -1358,9 +1335,9 @@ void i2CInitialize(uint8_t port, int32_t *status) {
 		return;
 	}
 
-	MUTEX_ID lock = port == 0 ? digitalI2COnBoardSemaphore:digitalI2CMXPSemaphore;
+	priority_recursive_mutex &lock = port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex;
 	{
-		Synchronized sync(lock);
+		std::unique_lock<priority_recursive_mutex> sync(lock);
 		if(port == 0) {
 			i2COnboardObjCount++;
 			if (i2COnBoardHandle > 0) return;
@@ -1405,10 +1382,10 @@ int32_t i2CTransaction(uint8_t port, uint8_t deviceAddress, uint8_t *dataToSend,
 		return true;
 	}*/
 	int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle;
-	MUTEX_ID lock = port == 0 ? digitalI2COnBoardSemaphore:digitalI2CMXPSemaphore;
+	priority_recursive_mutex &lock = port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex;
 
 	{
-		Synchronized sync(lock);
+		std::unique_lock<priority_recursive_mutex> sync(lock);
 		return i2clib_writeread(handle, deviceAddress, (const char*) dataToSend, (int32_t) sendSize, (char*) dataReceived, (int32_t) receiveSize);
 	}
 }
@@ -1435,9 +1412,9 @@ int32_t i2CWrite(uint8_t port, uint8_t deviceAddress, uint8_t* dataToSend, uint8
 		return true;
 	}*/
 	int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle;
-	MUTEX_ID lock = port == 0 ? digitalI2COnBoardSemaphore:digitalI2CMXPSemaphore;
+	priority_recursive_mutex &lock = port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex;
 	{
-		Synchronized sync(lock);
+		std::unique_lock<priority_recursive_mutex> sync(lock);
 		return i2clib_write(handle, deviceAddress, (const char*) dataToSend, (int32_t) sendSize);
 	}
 }
@@ -1472,9 +1449,9 @@ int32_t i2CRead(uint8_t port, uint8_t deviceAddress, uint8_t *buffer, uint8_t co
 			return true;
 		}*/
 	int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle;
-	MUTEX_ID lock = port == 0 ? digitalI2COnBoardSemaphore:digitalI2CMXPSemaphore;
+	priority_recursive_mutex &lock = port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex;
 	{
-		Synchronized sync(lock);
+		std::unique_lock<priority_recursive_mutex> sync(lock);
 		return i2clib_read(handle, deviceAddress, (char*) buffer, (int32_t) count);
 	}
 
@@ -1485,9 +1462,9 @@ void i2CClose(uint8_t port) {
 		//Set port out of range error here
 		return;
 	}
-	MUTEX_ID lock = port == 0 ? digitalI2COnBoardSemaphore:digitalI2CMXPSemaphore;
+	priority_recursive_mutex &lock = port == 0 ? digitalI2COnBoardMutex : digitalI2CMXPMutex;
 	{
-		Synchronized sync(lock);
+		std::unique_lock<priority_recursive_mutex> sync(lock);
 		if((port == 0 ? i2COnboardObjCount--:i2CMXPObjCount--) == 0) {
 			int32_t handle = port == 0 ? i2COnBoardHandle:i2CMXPHandle;
 			i2clib_close(handle);
diff --git a/hal/lib/Athena/Semaphore.cpp b/hal/lib/Athena/Semaphore.cpp
index 8d0162ed3f..b08727c2c5 100644
--- a/hal/lib/Athena/Semaphore.cpp
+++ b/hal/lib/Athena/Semaphore.cpp
@@ -45,8 +45,8 @@ MUTEX_ID initializeMutexNormal()
 
 void deleteMutex(MUTEX_ID sem)
 {
-	pthread_mutex_destroy(sem);
-	delete sem;
+       pthread_mutex_destroy(sem);
+       delete sem;
 }
 
 /**
@@ -69,7 +69,7 @@ int8_t tryTakeMutex(MUTEX_ID sem)
  */
 int8_t giveMutex(MUTEX_ID sem)
 {
-	return pthread_mutex_unlock(sem);
+       return pthread_mutex_unlock(sem);
 }
 
 SEMAPHORE_ID initializeSemaphore(uint32_t initial_value) {
@@ -134,5 +134,3 @@ int8_t takeMultiWait(MULTIWAIT_ID sem, MUTEX_ID m, int32_t timeout) {
 int8_t giveMultiWait(MULTIWAIT_ID sem) {
   return pthread_cond_broadcast(sem);
 }
-
-
diff --git a/hal/lib/Athena/Solenoid.cpp b/hal/lib/Athena/Solenoid.cpp
index c545106bb7..db699ff268 100644
--- a/hal/lib/Athena/Solenoid.cpp
+++ b/hal/lib/Athena/Solenoid.cpp
@@ -4,7 +4,6 @@
 #include "Port.h"
 #include "HAL/Errors.hpp"
 #include "ChipObject.h"
-#include "HAL/cpp/Synchronized.hpp"
 #include "NetworkCommunication/LoadOut.h"
 #include "ctre/PCM.h"
 
diff --git a/hal/lib/Athena/Task.cpp b/hal/lib/Athena/Task.cpp
index 28bc4772f0..688e3732ad 100644
--- a/hal/lib/Athena/Task.cpp
+++ b/hal/lib/Athena/Task.cpp
@@ -1,6 +1,13 @@
 
 #include "HAL/Task.hpp"
 
+#ifndef OK
+#define OK             0
+#endif /* OK */
+#ifndef ERROR
+#define ERROR          (-1)
+#endif /* ERROR */
+
 #include <stdio.h>
 #include <signal.h>
 
diff --git a/hal/lib/Athena/cpp/Resource.cpp b/hal/lib/Athena/cpp/Resource.cpp
index f24f3b1c83..f6ec0813ef 100644
--- a/hal/lib/Athena/cpp/Resource.cpp
+++ b/hal/lib/Athena/cpp/Resource.cpp
@@ -7,8 +7,9 @@
 #include "HAL/cpp/Resource.hpp"
 #include "HAL/Errors.hpp"
 #include <cstddef>
+#include "HAL/cpp/priority_mutex.h"
 
-ReentrantSemaphore Resource::m_createLock;
+priority_recursive_mutex Resource::m_createLock;
 
 /**
  * Allocate storage for a new instance of Resource.
@@ -17,7 +18,7 @@ ReentrantSemaphore Resource::m_createLock;
  */
 Resource::Resource(uint32_t elements)
 {
-	Synchronized sync(m_createLock);
+	std::unique_lock<priority_recursive_mutex> sync(m_createLock);
 	m_size = elements;
 	m_isAllocated = new bool[m_size];
 	for (uint32_t i=0; i < m_size; i++)
@@ -38,7 +39,7 @@ Resource::Resource(uint32_t elements)
  */
 /*static*/ void Resource::CreateResourceObject(Resource **r, uint32_t elements)
 {
-	Synchronized sync(m_createLock);
+	std::unique_lock<priority_recursive_mutex> sync(m_createLock);
 	if (*r == NULL)
 	{
 		*r = new Resource(elements);
@@ -61,7 +62,7 @@ Resource::~Resource()
  */
 uint32_t Resource::Allocate(const char *resourceDesc)
 {
-	Synchronized sync(m_allocateLock);
+	std::unique_lock<priority_recursive_mutex> sync(m_allocateLock);
 	for (uint32_t i=0; i < m_size; i++)
 	{
 		if (!m_isAllocated[i])
@@ -81,7 +82,7 @@ uint32_t Resource::Allocate(const char *resourceDesc)
  */
 uint32_t Resource::Allocate(uint32_t index, const char *resourceDesc)
 {
-	Synchronized sync(m_allocateLock);
+	std::unique_lock<priority_recursive_mutex> sync(m_allocateLock);
 	if (index >= m_size)
 	{
 		// TODO: wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc);
@@ -104,7 +105,7 @@ uint32_t Resource::Allocate(uint32_t index, const char *resourceDesc)
  */
 void Resource::Free(uint32_t index)
 {
-	Synchronized sync(m_allocateLock);
+	std::unique_lock<priority_recursive_mutex> sync(m_allocateLock);
 	if (index == ~0ul) return;
 	if (index >= m_size)
 	{
diff --git a/hal/lib/Athena/cpp/Semaphore.cpp b/hal/lib/Athena/cpp/Semaphore.cpp
new file mode 100644
index 0000000000..e92eccbccf
--- /dev/null
+++ b/hal/lib/Athena/cpp/Semaphore.cpp
@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "HAL/cpp/Semaphore.hpp"
+
+Semaphore::Semaphore(uint32_t count) {
+  m_count = count;
+}
+
+void Semaphore::give() {
+  std::unique_lock<priority_mutex> lock(m_mutex);
+  ++m_count;
+  m_condition.notify_one();
+}
+
+void Semaphore::take() {
+  std::unique_lock<priority_mutex> lock(m_mutex);
+  m_condition.wait(lock, [this] { return m_count; } );
+  --m_count;
+}
+
+bool Semaphore::tryTake() {
+  std::unique_lock<priority_mutex> lock(m_mutex);
+  if (m_count) {
+    --m_count;
+    return true;
+  }
+  else {
+    return false;
+  }
+}
diff --git a/hal/lib/Athena/cpp/Synchronized.cpp b/hal/lib/Athena/cpp/Synchronized.cpp
deleted file mode 100644
index e528208f80..0000000000
--- a/hal/lib/Athena/cpp/Synchronized.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
-/*----------------------------------------------------------------------------*/
-
-#include "HAL/cpp/Synchronized.hpp"
-#include "HAL/Semaphore.hpp"
-
-/**
- * Synchronized class deals with critical regions.
- * Declare a Synchronized object at the beginning of a block. That will take the semaphore.
- * When the code exits from the block it will call the destructor which will give the semaphore.
- * This ensures that no matter how the block is exited, the semaphore will always be released.
- * Use the CRITICAL_REGION(SEM_ID) and END_REGION macros to make the code look cleaner (see header file)
- * @param semaphore The semaphore controlling this critical region.
- */
-Synchronized::Synchronized(MUTEX_ID semaphore)
-{
-	m_mutex = semaphore;
-	m_semaphore = NULL;
-	takeMutex(m_mutex);
-}
-
-Synchronized::Synchronized(SEMAPHORE_ID semaphore)
-{
-	m_mutex = NULL;
-	m_semaphore = semaphore;
-	takeSemaphore(m_semaphore);
-}
-
-Synchronized::Synchronized(ReentrantSemaphore& semaphore)
-{
-	m_mutex = semaphore.m_semaphore;
-	m_semaphore = NULL;
-	takeMutex(m_mutex);
-}
-
-/**
- * This destructor unlocks the semaphore.
- */
-Synchronized::~Synchronized()
-{
-	if (m_mutex != NULL) {
-		giveMutex(m_mutex);
-	} else {
-		giveSemaphore(m_semaphore);
-	}
-}
diff --git a/hal/lib/Athena/cpp/priority_mutex.cpp b/hal/lib/Athena/cpp/priority_mutex.cpp
new file mode 100644
index 0000000000..66d3dcc48a
--- /dev/null
+++ b/hal/lib/Athena/cpp/priority_mutex.cpp
@@ -0,0 +1,33 @@
+#include "HAL/cpp/priority_mutex.h"
+
+void priority_recursive_mutex::lock() {
+  pthread_mutex_lock(&m_mutex);
+}
+
+void priority_recursive_mutex::unlock() {
+  pthread_mutex_unlock(&m_mutex);
+}
+
+bool priority_recursive_mutex::try_lock() noexcept {
+  return !pthread_mutex_trylock(&m_mutex);
+}
+
+pthread_mutex_t* priority_recursive_mutex::native_handle() {
+  return &m_mutex;
+}
+
+void priority_mutex::lock() {
+  pthread_mutex_lock(&m_mutex);
+}
+
+void priority_mutex::unlock() {
+  pthread_mutex_unlock(&m_mutex);
+}
+
+bool priority_mutex::try_lock() noexcept {
+  return !pthread_mutex_trylock(&m_mutex);
+}
+
+pthread_mutex_t* priority_mutex::native_handle() {
+  return &m_mutex;
+}
diff --git a/wpilibc/wpilibC++/include/Base.h b/wpilibc/wpilibC++/include/Base.h
index 301b1ea38d..8ad11df12e 100644
--- a/wpilibc/wpilibC++/include/Base.h
+++ b/wpilibc/wpilibC++/include/Base.h
@@ -5,12 +5,6 @@
 /*----------------------------------------------------------------------------*/
 #pragma once
 
-// If don't have C++11, define constexpr as const for WindRiver
-#if __cplusplus < 201103L
-#define constexpr const
-#define nullptr NULL
-#endif
-
 // A macro for making a class move-only
 #define DISALLOW_COPY_AND_ASSIGN(TypeName)       \
   TypeName(const TypeName&) = delete;            \
diff --git a/wpilibc/wpilibC++/include/Commands/Scheduler.h b/wpilibc/wpilibC++/include/Commands/Scheduler.h
index c794f4871b..ee085d91c4 100644
--- a/wpilibc/wpilibC++/include/Commands/Scheduler.h
+++ b/wpilibc/wpilibC++/include/Commands/Scheduler.h
@@ -15,11 +15,11 @@
 #include "networktables2/type/NumberArray.h"
 #include "networktables2/type/StringArray.h"
 #include "SmartDashboard/SmartDashboard.h"
-#include "HAL/Semaphore.hpp"
 #include <list>
 #include <map>
 #include <set>
 #include <vector>
+#include "HAL/cpp/priority_mutex.h"
 
 class ButtonScheduler;
 class Subsystem;
@@ -46,16 +46,16 @@ class Scheduler : public ErrorBase, public NamedSendable {
 
  private:
   Scheduler();
-  virtual ~Scheduler();
+  virtual ~Scheduler() = default;
 
   void ProcessCommandAddition(Command *command);
 
   Command::SubsystemSet m_subsystems;
-  MUTEX_ID m_buttonsLock = nullptr;
+  priority_mutex m_buttonsLock;
   typedef std::vector<ButtonScheduler *> ButtonVector;
   ButtonVector m_buttons;
   typedef std::vector<Command *> CommandVector;
-  MUTEX_ID m_additionsLock = nullptr;
+  priority_mutex m_additionsLock;
   CommandVector m_additions;
   typedef std::set<Command *> CommandSet;
   CommandSet m_commands;
diff --git a/wpilibc/wpilibC++/include/ErrorBase.h b/wpilibc/wpilibC++/include/ErrorBase.h
index d56251e98f..0ec96a3384 100644
--- a/wpilibc/wpilibC++/include/ErrorBase.h
+++ b/wpilibc/wpilibC++/include/ErrorBase.h
@@ -8,7 +8,8 @@
 
 #include "Base.h"
 #include "Error.h"
-#include "HAL/Semaphore.hpp"
+
+#include "HAL/cpp/priority_mutex.h"
 
 #define wpi_setErrnoErrorWithContext(context) \
   (this->SetErrnoError((context), __FILE__, __FUNCTION__, __LINE__))
@@ -81,7 +82,7 @@ class ErrorBase {
  protected:
   mutable Error m_error;
   // TODO: Replace globalError with a global list of all errors.
-  static MUTEX_ID _globalErrorMutex;
+  static priority_mutex _globalErrorMutex;
   static Error _globalError;
 
  private:
diff --git a/wpilibc/wpilibC++/include/Notifier.h b/wpilibc/wpilibC++/include/Notifier.h
index b034b12a84..0f0e4bda06 100644
--- a/wpilibc/wpilibC++/include/Notifier.h
+++ b/wpilibc/wpilibC++/include/Notifier.h
@@ -8,7 +8,7 @@
 
 #include "ErrorBase.h"
 #include "Task.h"
-#include "HAL/cpp/Synchronized.hpp"
+#include "HAL/cpp/priority_mutex.h"
 
 typedef void (*TimerEventHandler)(void *param);
 
@@ -22,7 +22,7 @@ class Notifier : public ErrorBase {
 
  private:
   static Notifier *timerQueueHead;
-  static ReentrantSemaphore queueSemaphore;
+  static priority_recursive_mutex queueMutex;
   static void *m_notifier;
   static int refcount;
 
@@ -40,7 +40,7 @@ class Notifier : public ErrorBase {
   Notifier *m_nextEvent = nullptr;    // next Nofifier event
   bool m_periodic = false;          // true if this is a periodic event
   bool m_queued = false;            // indicates if this entry is queued
-  SEMAPHORE_ID m_handlerSemaphore;  // held by interrupt manager task while
+  priority_mutex m_handlerMutex;  // held by interrupt manager task while
                                     // handler call is in progress
 
   DISALLOW_COPY_AND_ASSIGN(Notifier);
diff --git a/wpilibc/wpilibC++/include/PIDController.h b/wpilibc/wpilibC++/include/PIDController.h
index 135055ac9f..b04f374ea4 100644
--- a/wpilibc/wpilibC++/include/PIDController.h
+++ b/wpilibc/wpilibC++/include/PIDController.h
@@ -9,8 +9,8 @@
 #include "Base.h"
 #include "Controller.h"
 #include "LiveWindow/LiveWindow.h"
-#include "HAL/Semaphore.hpp"
 #include "PIDInterface.h"
+#include "HAL/cpp/priority_mutex.h"
 
 class PIDOutput;
 class PIDSource;
@@ -87,7 +87,7 @@ class PIDController : public LiveWindowSendable,
   float m_result = 0;
   float m_period;
 
-  MUTEX_ID m_semaphore = 0;
+  mutable priority_mutex m_mutex;
 
   PIDSource *m_pidInput;
   PIDOutput *m_pidOutput;
diff --git a/wpilibc/wpilibC++/include/PIDInterface.h b/wpilibc/wpilibC++/include/PIDInterface.h
index 049668f373..5d50199c1d 100644
--- a/wpilibc/wpilibC++/include/PIDInterface.h
+++ b/wpilibc/wpilibC++/include/PIDInterface.h
@@ -3,7 +3,6 @@
 #include "Base.h"
 #include "Controller.h"
 #include "LiveWindow/LiveWindow.h"
-#include "HAL/Semaphore.hpp"
 
 class PIDInterface : public Controller {
   virtual void SetPID(double p, double i, double d) = 0;
diff --git a/wpilibc/wpilibC++/include/Resource.h b/wpilibc/wpilibC++/include/Resource.h
index 8c8033ffc9..339ce04992 100644
--- a/wpilibc/wpilibC++/include/Resource.h
+++ b/wpilibc/wpilibC++/include/Resource.h
@@ -7,9 +7,10 @@
 #pragma once
 
 #include "ErrorBase.h"
-#include "HAL/cpp/Synchronized.hpp"
 #include <stdint.h>
 
+#include "HAL/cpp/priority_mutex.h"
+
 /**
  * The Resource class is a convenient way to track allocated resources.
  * It tracks them as indicies in the range [0 .. elements - 1].
@@ -31,10 +32,10 @@ class Resource : public ErrorBase {
   explicit Resource(uint32_t size);
 
   bool *m_isAllocated;
-  ReentrantSemaphore m_allocateLock;
+  priority_recursive_mutex m_allocateLock;
   uint32_t m_size;
 
-  static ReentrantSemaphore m_createLock;
+  static priority_recursive_mutex m_createLock;
 
   DISALLOW_COPY_AND_ASSIGN(Resource);
 };
diff --git a/wpilibc/wpilibC++/include/Timer.h b/wpilibc/wpilibC++/include/Timer.h
index bd914806b5..cef118d87c 100644
--- a/wpilibc/wpilibC++/include/Timer.h
+++ b/wpilibc/wpilibC++/include/Timer.h
@@ -6,8 +6,8 @@
 /*----------------------------------------------------------------------------*/
 #pragma once
 
-#include "HAL/Semaphore.hpp"
 #include "Base.h"
+#include "HAL/cpp/priority_mutex.h"
 
 typedef void (*TimerInterruptHandler)(void *param);
 
@@ -28,7 +28,7 @@ double GetTime();
 class Timer {
  public:
   Timer();
-  virtual ~Timer();
+  virtual ~Timer() = default;
   double Get() const;
   void Reset();
   void Start();
@@ -46,6 +46,6 @@ class Timer {
   double m_startTime = 0.0;
   double m_accumulatedTime = 0.0;
   bool m_running = false;
-  MUTEX_ID m_semaphore = nullptr;
+  mutable priority_mutex m_mutex;
   DISALLOW_COPY_AND_ASSIGN(Timer);
 };
diff --git a/wpilibc/wpilibC++/src/Commands/Scheduler.cpp b/wpilibc/wpilibC++/src/Commands/Scheduler.cpp
index c23785d2b6..f40fe92560 100644
--- a/wpilibc/wpilibC++/src/Commands/Scheduler.cpp
+++ b/wpilibc/wpilibC++/src/Commands/Scheduler.cpp
@@ -10,27 +10,15 @@
 #include "Buttons/ButtonScheduler.h"
 #include "Commands/Subsystem.h"
 #include "HLUsageReporting.h"
-#include "HAL/cpp/Synchronized.hpp"
 #include "WPIErrors.h"
 #include <iostream>
 #include <set>
 #include <algorithm>
 
 Scheduler::Scheduler() {
-  m_buttonsLock = initializeMutexNormal();
-  m_additionsLock = initializeMutexNormal();
-
   HLUsageReporting::ReportScheduler();
 }
 
-Scheduler::~Scheduler() {
-  takeMutex(m_additionsLock);
-  deleteMutex(m_additionsLock);
-
-  takeMutex(m_buttonsLock);
-  deleteMutex(m_buttonsLock);
-}
-
 /**
  * Returns the {@link Scheduler}, creating it if one does not exist.
  * @return the {@link Scheduler}
@@ -50,7 +38,7 @@ void Scheduler::SetEnabled(bool enabled) { m_enabled = enabled; }
  * @param command The command to be scheduled
  */
 void Scheduler::AddCommand(Command *command) {
-  Synchronized sync(m_additionsLock);
+  std::unique_lock<priority_mutex> sync(m_additionsLock);
   if (std::find(m_additions.begin(), m_additions.end(), command) !=
       m_additions.end())
     return;
@@ -58,7 +46,7 @@ void Scheduler::AddCommand(Command *command) {
 }
 
 void Scheduler::AddButton(ButtonScheduler *button) {
-  Synchronized sync(m_buttonsLock);
+  std::unique_lock<priority_mutex> sync(m_buttonsLock);
   m_buttons.push_back(button);
 }
 
@@ -122,7 +110,7 @@ void Scheduler::Run() {
   {
     if (!m_enabled) return;
 
-    Synchronized sync(m_buttonsLock);
+    std::unique_lock<priority_mutex> sync(m_buttonsLock);
     auto rButtonIter = m_buttons.rbegin();
     for (; rButtonIter != m_buttons.rend(); rButtonIter++) {
       (*rButtonIter)->Execute();
@@ -145,7 +133,7 @@ void Scheduler::Run() {
 
   // Add the new things
   {
-    Synchronized sync(m_additionsLock);
+    std::unique_lock<priority_mutex> sync(m_additionsLock);
     auto additionsIter = m_additions.begin();
     for (; additionsIter != m_additions.end(); additionsIter++) {
       ProcessCommandAddition(*additionsIter);
diff --git a/wpilibc/wpilibC++/src/ErrorBase.cpp b/wpilibc/wpilibC++/src/ErrorBase.cpp
index b7eae3d48b..adfe387b8d 100644
--- a/wpilibc/wpilibC++/src/ErrorBase.cpp
+++ b/wpilibc/wpilibC++/src/ErrorBase.cpp
@@ -6,7 +6,6 @@
 /*----------------------------------------------------------------------------*/
 
 #include "ErrorBase.h"
-#include "HAL/cpp/Synchronized.hpp"
 #define WPI_ERRORS_DEFINE_STRINGS
 #include "WPIErrors.h"
 
@@ -14,7 +13,7 @@
 #include <cstring>
 #include <cstdio>
 
-MUTEX_ID ErrorBase::_globalErrorMutex = initializeMutexNormal();
+priority_mutex ErrorBase::_globalErrorMutex;
 Error ErrorBase::_globalError;
 /**
  * @brief Initialize the instance status to 0 for now.
@@ -58,7 +57,7 @@ void ErrorBase::SetErrnoError(const char* contextMessage, const char* filename,
   m_error.Set(-1, err, filename, function, lineNumber, this);
 
   // Update the global error if there is not one already set.
-  Synchronized mutex(_globalErrorMutex);
+  std::unique_lock<priority_mutex> mutex(_globalErrorMutex);
   if (_globalError.GetCode() == 0) {
     _globalError.Clone(m_error);
   }
@@ -86,7 +85,7 @@ void ErrorBase::SetImaqError(int success, const char* contextMessage,
     m_error.Set(success, err, filename, function, lineNumber, this);
 
     // Update the global error if there is not one already set.
-    Synchronized mutex(_globalErrorMutex);
+    std::unique_lock<priority_mutex> mutex(_globalErrorMutex);
     if (_globalError.GetCode() == 0) {
       _globalError.Clone(m_error);
     }
@@ -111,7 +110,7 @@ void ErrorBase::SetError(Error::Code code, const char* contextMessage,
     m_error.Set(code, contextMessage, filename, function, lineNumber, this);
 
     // Update the global error if there is not one already set.
-    Synchronized mutex(_globalErrorMutex);
+    std::unique_lock<priority_mutex> mutex(_globalErrorMutex);
     if (_globalError.GetCode() == 0) {
       _globalError.Clone(m_error);
     }
@@ -137,7 +136,7 @@ void ErrorBase::SetWPIError(const char* errorMessage, Error::Code code,
   m_error.Set(code, err, filename, function, lineNumber, this);
 
   // Update the global error if there is not one already set.
-  Synchronized mutex(_globalErrorMutex);
+  std::unique_lock<priority_mutex> mutex(_globalErrorMutex);
   if (_globalError.GetCode() == 0) {
     _globalError.Clone(m_error);
   }
@@ -159,7 +158,7 @@ void ErrorBase::SetGlobalError(Error::Code code, const char* contextMessage,
                                uint32_t lineNumber) {
   //  If there was an error
   if (code != 0) {
-    Synchronized mutex(_globalErrorMutex);
+    std::unique_lock<priority_mutex> mutex(_globalErrorMutex);
 
     //  Set the current error information for this object.
     _globalError.Set(code, contextMessage, filename, function, lineNumber,
@@ -174,7 +173,7 @@ void ErrorBase::SetGlobalWPIError(const char* errorMessage,
   char err[256];
   sprintf(err, "%s: %s", errorMessage, contextMessage);
 
-  Synchronized mutex(_globalErrorMutex);
+  std::unique_lock<priority_mutex> mutex(_globalErrorMutex);
   if (_globalError.GetCode() != 0) {
     _globalError.Clear();
   }
@@ -185,6 +184,6 @@ void ErrorBase::SetGlobalWPIError(const char* errorMessage,
   * Retrieve the current global error.
 */
 Error& ErrorBase::GetGlobalError() {
-  Synchronized mutex(_globalErrorMutex);
+  std::unique_lock<priority_mutex> mutex(_globalErrorMutex);
   return _globalError;
 }
diff --git a/wpilibc/wpilibC++/src/Resource.cpp b/wpilibc/wpilibC++/src/Resource.cpp
index 492e809a31..296f47970d 100644
--- a/wpilibc/wpilibC++/src/Resource.cpp
+++ b/wpilibc/wpilibC++/src/Resource.cpp
@@ -9,7 +9,7 @@
 #include "WPIErrors.h"
 #include "ErrorBase.h"
 
-ReentrantSemaphore Resource::m_createLock;
+priority_recursive_mutex Resource::m_createLock;
 
 /**
  * Allocate storage for a new instance of Resource.
@@ -19,7 +19,7 @@ ReentrantSemaphore Resource::m_createLock;
  * 1].
  */
 Resource::Resource(uint32_t elements) {
-  Synchronized sync(m_createLock);
+  std::unique_lock<priority_recursive_mutex> sync(m_createLock);
   m_size = elements;
   m_isAllocated = new bool[m_size];
   for (uint32_t i = 0; i < m_size; i++) {
@@ -39,7 +39,7 @@ Resource::Resource(uint32_t elements) {
  */
 /*static*/ void Resource::CreateResourceObject(Resource **r,
                                                uint32_t elements) {
-  Synchronized sync(m_createLock);
+  std::unique_lock<priority_recursive_mutex> sync(m_createLock);
   if (*r == nullptr) {
     *r = new Resource(elements);
   }
@@ -59,7 +59,7 @@ Resource::~Resource() { delete[] m_isAllocated; }
  * within the range is located and returned after it is marked allocated.
  */
 uint32_t Resource::Allocate(const char *resourceDesc) {
-  Synchronized sync(m_allocateLock);
+  std::unique_lock<priority_recursive_mutex> sync(m_allocateLock);
   for (uint32_t i = 0; i < m_size; i++) {
     if (!m_isAllocated[i]) {
       m_isAllocated[i] = true;
@@ -77,7 +77,7 @@ uint32_t Resource::Allocate(const char *resourceDesc) {
  * unallocated, then returned.
  */
 uint32_t Resource::Allocate(uint32_t index, const char *resourceDesc) {
-  Synchronized sync(m_allocateLock);
+  std::unique_lock<priority_recursive_mutex> sync(m_allocateLock);
   if (index >= m_size) {
     wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc);
     return ~0ul;
@@ -98,7 +98,7 @@ uint32_t Resource::Allocate(uint32_t index, const char *resourceDesc) {
  * else in the program.
  */
 void Resource::Free(uint32_t index) {
-  Synchronized sync(m_allocateLock);
+  std::unique_lock<priority_recursive_mutex> sync(m_allocateLock);
   if (index == ~0ul) return;
   if (index >= m_size) {
     wpi_setWPIError(NotAllocated);
diff --git a/wpilibc/wpilibC++Devices/include/CANJaguar.h b/wpilibc/wpilibC++Devices/include/CANJaguar.h
index 02c5208c66..068ebe630f 100644
--- a/wpilibc/wpilibC++Devices/include/CANJaguar.h
+++ b/wpilibc/wpilibC++Devices/include/CANJaguar.h
@@ -11,7 +11,7 @@
 #include "MotorSafetyHelper.h"
 #include "PIDOutput.h"
 #include "CANSpeedController.h"
-#include "HAL/Semaphore.hpp"
+#include "HAL/cpp/Semaphore.hpp"
 #include "HAL/HAL.hpp"
 #include "LiveWindow/LiveWindowSendable.h"
 #include "tables/ITable.h"
@@ -19,7 +19,7 @@
 #include "CAN/can_proto.h"
 
 #include <atomic>
-#include <mutex>
+#include "HAL/cpp/priority_mutex.h"
 #include <utility>
 
 /**
@@ -168,7 +168,7 @@ class CANJaguar : public MotorSafety,
   void setupPeriodicStatus();
   void updatePeriodicStatus() const;
 
-  mutable std::recursive_mutex m_mutex;
+  mutable priority_recursive_mutex m_mutex;
 
   uint8_t m_deviceNumber;
   float m_value = 0.0f;
diff --git a/wpilibc/wpilibC++Devices/include/CameraServer.h b/wpilibc/wpilibC++Devices/include/CameraServer.h
index 3466ab4663..17ece99829 100644
--- a/wpilibc/wpilibC++Devices/include/CameraServer.h
+++ b/wpilibc/wpilibC++Devices/include/CameraServer.h
@@ -11,7 +11,7 @@
 #include "nivision.h"
 #include "NIIMAQdx.h"
 
-#include <mutex>
+#include "HAL/cpp/priority_mutex.h"
 #include <thread>
 #include <memory>
 #include <condition_variable>
@@ -34,7 +34,7 @@ class CameraServer : public ErrorBase {
   std::shared_ptr<USBCamera> m_camera;
   std::thread m_serverThread;
   std::thread m_captureThread;
-  std::recursive_mutex m_imageMutex;
+  priority_recursive_mutex m_imageMutex;
   std::condition_variable_any m_newImageVariable;
   std::vector<uint8_t*> m_dataPool;
   unsigned int m_quality;
diff --git a/wpilibc/wpilibC++Devices/include/DriverStation.h b/wpilibc/wpilibC++Devices/include/DriverStation.h
index be3c368081..f98177d648 100644
--- a/wpilibc/wpilibC++Devices/include/DriverStation.h
+++ b/wpilibc/wpilibC++Devices/include/DriverStation.h
@@ -9,6 +9,10 @@
 #include "RobotState.h"
 #include "Task.h"
 #include "HAL/HAL.hpp"
+#include "HAL/cpp/Semaphore.hpp"
+#include "HAL/cpp/priority_mutex.h"
+#include "HAL/cpp/priority_condition_variable.h"
+#include <condition_variable>
 
 struct HALControlWord;
 class AnalogInput;
@@ -98,11 +102,11 @@ class DriverStation : public SensorBase, public RobotStateInterface {
   HALJoystickButtons m_joystickButtons[kJoystickPorts];
   HALJoystickDescriptor m_joystickDescriptor[kJoystickPorts];
   Task m_task{"DriverStation", (FUNCPTR)DriverStation::InitTask};
-  SEMAPHORE_ID m_newControlData = 0;
-  MULTIWAIT_ID m_packetDataAvailableMultiWait = 0;
-  MUTEX_ID m_packetDataAvailableMutex;
-  MULTIWAIT_ID m_waitForDataSem = 0;
-  MUTEX_ID m_waitForDataMutex;
+  mutable Semaphore m_newControlData{Semaphore::kEmpty};
+  mutable priority_condition_variable m_packetDataAvailableCond;
+  priority_mutex m_packetDataAvailableMutex;
+  std::condition_variable_any m_waitForDataCond;
+  priority_mutex m_waitForDataMutex;
   bool m_userInDisabled = false;
   bool m_userInAutonomous = false;
   bool m_userInTeleop = false;
diff --git a/wpilibc/wpilibC++Devices/include/MotorSafetyHelper.h b/wpilibc/wpilibC++Devices/include/MotorSafetyHelper.h
index 173734da87..8381567c60 100644
--- a/wpilibc/wpilibC++Devices/include/MotorSafetyHelper.h
+++ b/wpilibc/wpilibC++Devices/include/MotorSafetyHelper.h
@@ -7,7 +7,7 @@
 #pragma once
 
 #include "ErrorBase.h"
-#include "HAL/cpp/Synchronized.hpp"
+#include "HAL/cpp/priority_mutex.h"
 
 class MotorSafety;
 
@@ -28,13 +28,13 @@ class MotorSafetyHelper : public ErrorBase {
   double m_expiration;  // the expiration time for this object
   bool m_enabled;       // true if motor safety is enabled for this motor
   double m_stopTime;    // the FPGA clock value when this motor has expired
-  mutable ReentrantSemaphore
+  mutable priority_recursive_mutex
       m_syncMutex;            // protect accesses to the state for this object
   MotorSafety *m_safeObject;  // the object that is using the helper
   MotorSafetyHelper *
       m_nextHelper;  // next object in the list of MotorSafetyHelpers
   static MotorSafetyHelper *
       m_headHelper;  // the head of the list of MotorSafetyHelper objects
-  static ReentrantSemaphore
+  static priority_recursive_mutex
       m_listMutex;  // protect accesses to the list of helpers
 };
diff --git a/wpilibc/wpilibC++Devices/include/Preferences.h b/wpilibc/wpilibC++Devices/include/Preferences.h
index 9464b2b389..82afa02a2e 100644
--- a/wpilibc/wpilibC++Devices/include/Preferences.h
+++ b/wpilibc/wpilibC++Devices/include/Preferences.h
@@ -9,9 +9,9 @@
 #include "ErrorBase.h"
 #include "Task.h"
 #include <map>
-#include "HAL/Semaphore.hpp"
 #include <string>
 #include <vector>
+#include "HAL/cpp/Semaphore.hpp"
 #include "tables/ITableListener.h"
 #include "networktables/NetworkTable.h"
 
@@ -64,7 +64,7 @@ class Preferences : public ErrorBase, public ITableListener {
 
  protected:
   Preferences();
-  virtual ~Preferences();
+  virtual ~Preferences() = default;
 
  private:
   std::string Get(const char *key);
@@ -83,11 +83,11 @@ class Preferences : public ErrorBase, public ITableListener {
   }
 
   /** The semaphore for accessing the file */
-  MUTEX_ID m_fileLock = nullptr;
+  priority_recursive_mutex m_fileLock;
   /** The semaphore for beginning reads and writes to the file */
-  SEMAPHORE_ID m_fileOpStarted = nullptr;
+  Semaphore m_fileOpStarted{Semaphore::kEmpty};
   /** The semaphore for reading from the table */
-  MUTEX_ID m_tableLock = nullptr;
+  priority_recursive_mutex m_tableLock;
   typedef std::map<std::string, std::string> StringMap;
   /** The actual values (String->String) */
   StringMap m_values;
diff --git a/wpilibc/wpilibC++Devices/include/SolenoidBase.h b/wpilibc/wpilibC++Devices/include/SolenoidBase.h
index 0a51be6352..3c864a2186 100644
--- a/wpilibc/wpilibC++Devices/include/SolenoidBase.h
+++ b/wpilibc/wpilibC++Devices/include/SolenoidBase.h
@@ -9,7 +9,6 @@
 #include "Resource.h"
 #include "SensorBase.h"
 #include "HAL/HAL.hpp"
-#include "HAL/cpp/Synchronized.hpp"
 #include "Port.h"
 
 /**
diff --git a/wpilibc/wpilibC++Devices/include/USBCamera.h b/wpilibc/wpilibC++Devices/include/USBCamera.h
index 991c38bc90..d02c5d35aa 100644
--- a/wpilibc/wpilibC++Devices/include/USBCamera.h
+++ b/wpilibc/wpilibC++Devices/include/USBCamera.h
@@ -9,8 +9,8 @@
 #include "ErrorBase.h"
 #include "nivision.h"
 #include "NIIMAQdx.h"
+#include "HAL/cpp/priority_mutex.h"
 
-#include <mutex>
 #include <string>
 
 typedef enum whiteBalance_enum {
@@ -47,7 +47,7 @@ class USBCamera : public ErrorBase {
   bool m_active = false;
   bool m_open = false;
 
-  std::recursive_mutex m_mutex;
+  priority_recursive_mutex m_mutex;
 
   unsigned int m_width = 320;
   unsigned int m_height = 240;
diff --git a/wpilibc/wpilibC++Devices/include/Ultrasonic.h b/wpilibc/wpilibC++Devices/include/Ultrasonic.h
index f956fb1edc..620d57b89d 100644
--- a/wpilibc/wpilibC++Devices/include/Ultrasonic.h
+++ b/wpilibc/wpilibC++Devices/include/Ultrasonic.h
@@ -11,6 +11,8 @@
 #include "PIDSource.h"
 #include "LiveWindow/LiveWindowSendable.h"
 
+#include "HAL/cpp/priority_mutex.h"
+
 class Counter;
 class DigitalInput;
 class DigitalOutput;
@@ -80,7 +82,7 @@ class Ultrasonic : public SensorBase,
   static Task m_task;  // task doing the round-robin automatic sensing
   static Ultrasonic *m_firstSensor;  // head of the ultrasonic sensor list
   static bool m_automaticEnabled;    // automatic round robin mode
-  static SEMAPHORE_ID m_semaphore;  // synchronize access to the list of sensors
+  static priority_mutex m_mutex;  // synchronize access to the list of sensors
 
   DigitalInput *m_echoChannel;
   DigitalOutput *m_pingChannel;
diff --git a/wpilibc/wpilibC++Devices/include/Vision/AxisCamera.h b/wpilibc/wpilibC++Devices/include/Vision/AxisCamera.h
index 99e1baec5e..7cb62b83d3 100644
--- a/wpilibc/wpilibC++Devices/include/Vision/AxisCamera.h
+++ b/wpilibc/wpilibC++Devices/include/Vision/AxisCamera.h
@@ -8,7 +8,7 @@
 
 #include <thread>
 #include <string>
-#include <mutex>
+#include "HAL/cpp/priority_mutex.h"
 
 #include "ErrorBase.h"
 #include "Vision/ColorImage.h"
@@ -90,9 +90,9 @@ class AxisCamera : public ErrorBase {
   std::thread m_captureThread;
   std::string m_cameraHost;
   int m_cameraSocket = -1;
-  std::mutex m_captureMutex;
+  priority_mutex m_captureMutex;
 
-  std::mutex m_imageDataMutex;
+  priority_mutex m_imageDataMutex;
   std::vector<uint8_t> m_imageData;
   bool m_freshImage = false;
 
@@ -107,7 +107,7 @@ class AxisCamera : public ErrorBase {
   Rotation m_rotation = kRotation_0;
   bool m_parametersDirty = true;
   bool m_streamDirty = true;
-  std::mutex m_parametersMutex;
+  priority_mutex m_parametersMutex;
 
   bool m_done = false;
 
diff --git a/wpilibc/wpilibC++Devices/include/WPILib.h b/wpilibc/wpilibC++Devices/include/WPILib.h
index 7a1528c886..325dac19d1 100644
--- a/wpilibc/wpilibC++Devices/include/WPILib.h
+++ b/wpilibc/wpilibC++Devices/include/WPILib.h
@@ -77,7 +77,6 @@
 #include "Solenoid.h"
 #include "SpeedController.h"
 #include "SPI.h"
-#include "HAL/cpp/Synchronized.hpp"
 #include "Talon.h"
 #include "TalonSRX.h"
 #include "Task.h"
diff --git a/wpilibc/wpilibC++Devices/src/CANJaguar.cpp b/wpilibc/wpilibC++Devices/src/CANJaguar.cpp
index c99994baf9..442c326b4b 100644
--- a/wpilibc/wpilibC++Devices/src/CANJaguar.cpp
+++ b/wpilibc/wpilibC++Devices/src/CANJaguar.cpp
@@ -1551,7 +1551,7 @@ CANJaguar::ControlMode CANJaguar::GetControlMode() const {
  */
 float CANJaguar::GetBusVoltage() const {
   updatePeriodicStatus();
-  std::lock_guard<std::recursive_mutex> lock(m_mutex);
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 
   return m_busVoltage;
 }
@@ -1563,7 +1563,7 @@ float CANJaguar::GetBusVoltage() const {
  */
 float CANJaguar::GetOutputVoltage() const {
   updatePeriodicStatus();
-  std::lock_guard<std::recursive_mutex> lock(m_mutex);
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 
   return m_outputVoltage;
 }
@@ -1575,7 +1575,7 @@ float CANJaguar::GetOutputVoltage() const {
  */
 float CANJaguar::GetOutputCurrent() const {
   updatePeriodicStatus();
-  std::lock_guard<std::recursive_mutex> lock(m_mutex);
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 
   return m_outputCurrent;
 }
@@ -1587,7 +1587,7 @@ float CANJaguar::GetOutputCurrent() const {
  */
 float CANJaguar::GetTemperature() const {
   updatePeriodicStatus();
-  std::lock_guard<std::recursive_mutex> lock(m_mutex);
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 
   return m_temperature;
 }
@@ -1602,7 +1602,7 @@ float CANJaguar::GetTemperature() const {
  */
 double CANJaguar::GetPosition() const {
   updatePeriodicStatus();
-  std::lock_guard<std::recursive_mutex> lock(m_mutex);
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 
   return m_position;
 }
@@ -1614,7 +1614,7 @@ double CANJaguar::GetPosition() const {
  */
 double CANJaguar::GetSpeed() const {
   updatePeriodicStatus();
-  std::lock_guard<std::recursive_mutex> lock(m_mutex);
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 
   return m_speed;
 }
@@ -1626,7 +1626,7 @@ double CANJaguar::GetSpeed() const {
  */
 bool CANJaguar::GetForwardLimitOK() const {
   updatePeriodicStatus();
-  std::lock_guard<std::recursive_mutex> lock(m_mutex);
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 
   return m_limits & kForwardLimit;
 }
@@ -1638,7 +1638,7 @@ bool CANJaguar::GetForwardLimitOK() const {
  */
 bool CANJaguar::GetReverseLimitOK() const {
   updatePeriodicStatus();
-  std::lock_guard<std::recursive_mutex> lock(m_mutex);
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 
   return m_limits & kReverseLimit;
 }
@@ -1654,7 +1654,7 @@ bool CANJaguar::GetReverseLimitOK() const {
  */
 uint16_t CANJaguar::GetFaults() const {
   updatePeriodicStatus();
-  std::lock_guard<std::recursive_mutex> lock(m_mutex);
+  std::lock_guard<priority_recursive_mutex> lock(m_mutex);
 
   return m_faults;
 }
diff --git a/wpilibc/wpilibC++Devices/src/CameraServer.cpp b/wpilibc/wpilibC++Devices/src/CameraServer.cpp
index 847fd2bab2..fad86586f4 100644
--- a/wpilibc/wpilibC++Devices/src/CameraServer.cpp
+++ b/wpilibc/wpilibC++Devices/src/CameraServer.cpp
@@ -35,14 +35,14 @@ void CameraServer::FreeImageData(
   if (std::get<3>(imageData))
     imaqDispose(std::get<0>(imageData));
   else if (std::get<0>(imageData) != nullptr) {
-    std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+    std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
     m_dataPool.push_back(std::get<0>(imageData));
   }
 }
 
 void CameraServer::SetImageData(uint8_t* data, unsigned int size,
                                 unsigned int start, bool imaqData) {
-  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
   FreeImageData(m_imageData);
   m_imageData = std::make_tuple(data, size, start, imaqData);
   m_newImageVariable.notify_all();
@@ -58,7 +58,7 @@ void CameraServer::SetImage(Image const* image) {
   bool hwClient;
   {
     // Make a local copy of the hwClient variable so that we can safely use it.
-    std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+    std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
     hwClient = m_hwClient;
   }
   unsigned int start = 0;
@@ -83,7 +83,7 @@ void CameraServer::AutoCapture() {
     bool hwClient;
     uint8_t* data = nullptr;
     {
-      std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+      std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
       hwClient = m_hwClient;
       if (hwClient) {
         data = m_dataPool.back();
@@ -109,7 +109,7 @@ void CameraServer::StartAutomaticCapture(char const* cameraName) {
 }
 
 void CameraServer::StartAutomaticCapture(std::shared_ptr<USBCamera> camera) {
-  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
   if (m_autoCaptureStarted) return;
 
   m_camera = camera;
@@ -121,12 +121,12 @@ void CameraServer::StartAutomaticCapture(std::shared_ptr<USBCamera> camera) {
 }
 
 bool CameraServer::IsAutoCaptureStarted() {
-  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
   return m_autoCaptureStarted;
 }
 
 void CameraServer::SetSize(unsigned int size) {
-  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
   if (!m_camera) return;
   if (size == kSize160x120)
     m_camera->SetSize(160, 120);
@@ -137,12 +137,12 @@ void CameraServer::SetSize(unsigned int size) {
 }
 
 void CameraServer::SetQuality(unsigned int quality) {
-  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
   m_quality = quality > 100 ? 100 : quality;
 }
 
 unsigned int CameraServer::GetQuality() {
-  std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
   return m_quality;
 }
 
@@ -201,7 +201,7 @@ void CameraServer::Serve() {
 
     {
       // Wait for the camera to be setw
-      std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+      std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
       if (!m_camera) {
         std::cout << "Camera not yet ready, awaiting first image" << std::endl;
         m_newImageVariable.wait(lock);
@@ -219,7 +219,7 @@ void CameraServer::Serve() {
       auto startTime = std::chrono::steady_clock::now();
       std::tuple<uint8_t*, unsigned int, unsigned int, bool> imageData;
       {
-        std::unique_lock<std::recursive_mutex> lock(m_imageMutex);
+        std::unique_lock<priority_recursive_mutex> lock(m_imageMutex);
         m_newImageVariable.wait(lock);
         imageData = m_imageData;
         m_imageData = std::make_tuple<uint8_t*>(nullptr, 0, 0, false);
diff --git a/wpilibc/wpilibC++Devices/src/DriverStation.cpp b/wpilibc/wpilibC++Devices/src/DriverStation.cpp
index e072758c4f..a72944bf6a 100644
--- a/wpilibc/wpilibC++Devices/src/DriverStation.cpp
+++ b/wpilibc/wpilibC++Devices/src/DriverStation.cpp
@@ -7,7 +7,6 @@
 
 #include "DriverStation.h"
 #include "AnalogInput.h"
-#include "HAL/cpp/Synchronized.hpp"
 #include "Timer.h"
 #include "NetworkCommunication/FRCComm.h"
 #include "NetworkCommunication/UsageReporting.h"
@@ -45,19 +44,9 @@ DriverStation::DriverStation() {
     m_joystickDescriptor[i].type = -1;
     m_joystickDescriptor[i].name[0] = '\0';
   }
-  // Create a new semaphore
-  m_packetDataAvailableMultiWait = initializeMultiWait();
-  m_newControlData = initializeSemaphore(SEMAPHORE_EMPTY);
-
-  m_waitForDataSem = initializeMultiWait();
-  m_waitForDataMutex = initializeMutexNormal();
-
-  m_packetDataAvailableMultiWait = initializeMultiWait();
-  m_packetDataAvailableMutex = initializeMutexNormal();
-
   // Register that semaphore with the network communications task.
   // It will signal when new packet data is available.
-  HALSetNewDataSem(m_packetDataAvailableMultiWait);
+  HALSetNewDataSem(m_packetDataAvailableCond.native_handle());
 
   AddToSingletonList();
 
@@ -72,12 +61,8 @@ DriverStation::DriverStation() {
 
 DriverStation::~DriverStation() {
   m_task.Stop();
-  deleteMultiWait(m_waitForDataSem);
   // Unregister our semaphore.
   HALSetNewDataSem(nullptr);
-  deleteMultiWait(m_packetDataAvailableMultiWait);
-  deleteMutex(m_packetDataAvailableMutex);
-  deleteMutex(m_waitForDataMutex);
 }
 
 // XXX: This assumes that the calling convention treats pointers and uint32_ts
@@ -87,10 +72,12 @@ void DriverStation::InitTask(DriverStation* ds) { ds->Run(); }
 void DriverStation::Run() {
   int period = 0;
   while (true) {
-    takeMultiWait(m_packetDataAvailableMultiWait, m_packetDataAvailableMutex,
-                  0);
+    {
+      std::unique_lock<priority_mutex> lock(m_packetDataAvailableMutex);
+      m_packetDataAvailableCond.wait(lock);
+    }
     GetData();
-    giveMultiWait(m_waitForDataSem);
+    m_waitForDataCond.notify_all();
 
     if (++period >= 4) {
       MotorSafetyHelper::CheckMotors();
@@ -126,7 +113,7 @@ void DriverStation::GetData() {
     HALGetJoystickButtons(stick, &m_joystickButtons[stick]);
     HALGetJoystickDescriptor(stick, &m_joystickDescriptor[stick]);
   }
-  giveSemaphore(m_newControlData);
+  m_newControlData.give();
 }
 
 /**
@@ -454,7 +441,7 @@ bool DriverStation::IsSysBrownedOut() const {
  * @return True if the control data has been updated since the last call.
  */
 bool DriverStation::IsNewControlData() const {
-  return tryTakeSemaphore(m_newControlData) == 0;
+  return m_newControlData.tryTake() == false;
 }
 
 /**
@@ -520,7 +507,8 @@ uint32_t DriverStation::GetLocation() const {
  * to act on
  */
 void DriverStation::WaitForData() {
-  takeMultiWait(m_waitForDataSem, m_waitForDataMutex, SEMAPHORE_WAIT_FOREVER);
+  std::unique_lock<priority_mutex> lock(m_waitForDataMutex);
+  m_waitForDataCond.wait(lock);
 }
 
 /**
diff --git a/wpilibc/wpilibC++Devices/src/MotorSafetyHelper.cpp b/wpilibc/wpilibC++Devices/src/MotorSafetyHelper.cpp
index 9271ee7b3b..68b5806493 100644
--- a/wpilibc/wpilibC++Devices/src/MotorSafetyHelper.cpp
+++ b/wpilibc/wpilibC++Devices/src/MotorSafetyHelper.cpp
@@ -15,7 +15,7 @@
 #include <stdio.h>
 
 MotorSafetyHelper *MotorSafetyHelper::m_headHelper = nullptr;
-ReentrantSemaphore MotorSafetyHelper::m_listMutex;
+priority_recursive_mutex MotorSafetyHelper::m_listMutex;
 
 /**
  * The constructor for a MotorSafetyHelper object.
@@ -36,13 +36,13 @@ MotorSafetyHelper::MotorSafetyHelper(MotorSafety *safeObject) {
   m_expiration = DEFAULT_SAFETY_EXPIRATION;
   m_stopTime = Timer::GetFPGATimestamp();
 
-  Synchronized sync(m_listMutex);
+  std::unique_lock<priority_recursive_mutex> sync(m_listMutex);
   m_nextHelper = m_headHelper;
   m_headHelper = this;
 }
 
 MotorSafetyHelper::~MotorSafetyHelper() {
-  Synchronized sync(m_listMutex);
+  std::unique_lock<priority_recursive_mutex> sync(m_listMutex);
   if (m_headHelper == this) {
     m_headHelper = m_nextHelper;
   } else {
@@ -58,7 +58,7 @@ MotorSafetyHelper::~MotorSafetyHelper() {
  * Resets the timer on this object that is used to do the timeouts.
  */
 void MotorSafetyHelper::Feed() {
-  Synchronized sync(m_syncMutex);
+  std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
   m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
 }
 
@@ -67,7 +67,7 @@ void MotorSafetyHelper::Feed() {
  * @param expirationTime The timeout value in seconds.
  */
 void MotorSafetyHelper::SetExpiration(float expirationTime) {
-  Synchronized sync(m_syncMutex);
+  std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
   m_expiration = expirationTime;
 }
 
@@ -76,7 +76,7 @@ void MotorSafetyHelper::SetExpiration(float expirationTime) {
  * @return the timeout value in seconds.
  */
 float MotorSafetyHelper::GetExpiration() const {
-  Synchronized sync(m_syncMutex);
+  std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
   return m_expiration;
 }
 
@@ -86,7 +86,7 @@ float MotorSafetyHelper::GetExpiration() const {
  * timed out.
  */
 bool MotorSafetyHelper::IsAlive() const {
-  Synchronized sync(m_syncMutex);
+  std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
   return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
 }
 
@@ -102,7 +102,7 @@ void MotorSafetyHelper::Check() {
   DriverStation *ds = DriverStation::GetInstance();
   if (!m_enabled || ds->IsDisabled() || ds->IsTest()) return;
 
-  Synchronized sync(m_syncMutex);
+  std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
   if (m_stopTime < Timer::GetFPGATimestamp()) {
     char buf[128];
     char desc[64];
@@ -119,7 +119,7 @@ void MotorSafetyHelper::Check() {
  * @param enabled True if motor safety is enforced for this object
  */
 void MotorSafetyHelper::SetSafetyEnabled(bool enabled) {
-  Synchronized sync(m_syncMutex);
+  std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
   m_enabled = enabled;
 }
 
@@ -129,7 +129,7 @@ void MotorSafetyHelper::SetSafetyEnabled(bool enabled) {
  * @return True if motor safety is enforced for this device
  */
 bool MotorSafetyHelper::IsSafetyEnabled() const {
-  Synchronized sync(m_syncMutex);
+  std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
   return m_enabled;
 }
 
@@ -140,7 +140,7 @@ bool MotorSafetyHelper::IsSafetyEnabled() const {
  * timed out.
  */
 void MotorSafetyHelper::CheckMotors() {
-  Synchronized sync(m_listMutex);
+  std::unique_lock<priority_recursive_mutex> sync(m_listMutex);
   for (MotorSafetyHelper *msh = m_headHelper; msh != nullptr;
        msh = msh->m_nextHelper) {
     msh->Check();
diff --git a/wpilibc/wpilibC++Devices/src/Notifier.cpp b/wpilibc/wpilibC++Devices/src/Notifier.cpp
index edfa08f19c..19016107c7 100644
--- a/wpilibc/wpilibC++Devices/src/Notifier.cpp
+++ b/wpilibc/wpilibC++Devices/src/Notifier.cpp
@@ -12,7 +12,7 @@
 #include "HAL/HAL.hpp"
 
 Notifier *Notifier::timerQueueHead = nullptr;
-ReentrantSemaphore Notifier::queueSemaphore;
+priority_recursive_mutex Notifier::queueMutex;
 void *Notifier::m_notifier = nullptr;
 int Notifier::refcount = 0;
 
@@ -26,9 +26,8 @@ Notifier::Notifier(TimerEventHandler handler, void *param) {
     wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
   m_handler = handler;
   m_param = param;
-  m_handlerSemaphore = initializeSemaphore(SEMAPHORE_FULL);
   {
-    Synchronized sync(queueSemaphore);
+    std::unique_lock<priority_recursive_mutex> sync(queueMutex);
     // do the first time intialization of static variables
     if (refcount == 0) {
       int32_t status = 0;
@@ -46,7 +45,7 @@ Notifier::Notifier(TimerEventHandler handler, void *param) {
  */
 Notifier::~Notifier() {
   {
-    Synchronized sync(queueSemaphore);
+    std::unique_lock<priority_recursive_mutex> sync(queueMutex);
     DeleteFromQueue();
 
     // Delete the static variables when the last one is going away
@@ -57,11 +56,9 @@ Notifier::~Notifier() {
     }
   }
 
-  // Acquire the semaphore; this makes certain that the handler is
+  // Acquire the mutex; this makes certain that the handler is
   // not being executed by the interrupt manager.
-  takeSemaphore(m_handlerSemaphore);
-  // Delete while holding the semaphore so there can be no race.
-  deleteSemaphore(m_handlerSemaphore);
+  std::unique_lock<priority_mutex> lock(m_handlerMutex);
 }
 
 /**
@@ -96,7 +93,7 @@ void Notifier::ProcessQueue(uint32_t mask, void *params) {
   while (true)  // keep processing past events until no more
   {
     {
-      Synchronized sync(queueSemaphore);
+      std::unique_lock<priority_recursive_mutex> sync(queueMutex);
       double currentTime = GetClock();
       current = timerQueueHead;
       if (current == nullptr || current->m_expirationTime > currentTime) {
@@ -112,16 +109,16 @@ void Notifier::ProcessQueue(uint32_t mask, void *params) {
         // not periodic; removed from queue
         current->m_queued = false;
       }
-      // Take handler semaphore while holding queue semaphore to make sure
+      // Take handler mutex while holding queue mutex to make sure
       //  the handler will execute to completion in case we are being deleted.
-      takeSemaphore(current->m_handlerSemaphore);
+      current->m_handlerMutex.lock();
     }
 
     current->m_handler(current->m_param);  // call the event handler
-    giveSemaphore(current->m_handlerSemaphore);
+    current->m_handlerMutex.unlock();
   }
   // reschedule the first item in the queue
-  Synchronized sync(queueSemaphore);
+  std::unique_lock<priority_recursive_mutex> sync(queueMutex);
   UpdateAlarm();
 }
 
@@ -206,7 +203,7 @@ void Notifier::DeleteFromQueue() {
  * @param delay Seconds to wait before the handler is called.
  */
 void Notifier::StartSingle(double delay) {
-  Synchronized sync(queueSemaphore);
+  std::unique_lock<priority_recursive_mutex> sync(queueMutex);
   m_periodic = false;
   m_period = delay;
   DeleteFromQueue();
@@ -222,7 +219,7 @@ void Notifier::StartSingle(double delay) {
  * the call to this method.
  */
 void Notifier::StartPeriodic(double period) {
-  Synchronized sync(queueSemaphore);
+  std::unique_lock<priority_recursive_mutex> sync(queueMutex);
   m_periodic = true;
   m_period = period;
   DeleteFromQueue();
@@ -240,10 +237,10 @@ void Notifier::StartPeriodic(double period) {
  */
 void Notifier::Stop() {
   {
-    Synchronized sync(queueSemaphore);
+    std::unique_lock<priority_recursive_mutex> sync(queueMutex);
     DeleteFromQueue();
   }
   // Wait for a currently executing handler to complete before returning from
   // Stop()
-  Synchronized sync(m_handlerSemaphore);
+  std::unique_lock<priority_mutex> sync(m_handlerMutex);
 }
diff --git a/wpilibc/wpilibC++Devices/src/PIDController.cpp b/wpilibc/wpilibC++Devices/src/PIDController.cpp
index 4555c3830c..d7d62cc24b 100644
--- a/wpilibc/wpilibC++Devices/src/PIDController.cpp
+++ b/wpilibc/wpilibC++Devices/src/PIDController.cpp
@@ -11,7 +11,6 @@
 #include "PIDOutput.h"
 #include <math.h>
 #include <vector>
-#include "HAL/cpp/Synchronized.hpp"
 #include "HAL/HAL.hpp"
 
 static const char *kP = "p";
@@ -56,8 +55,6 @@ PIDController::PIDController(float Kp, float Ki, float Kd, float Kf,
 void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
                                PIDSource *source, PIDOutput *output,
                                float period) {
-  m_semaphore = initializeMutexNormal();
-
   m_controlLoop = new Notifier(PIDController::CallCalculate, this);
 
   m_P = Kp;
@@ -80,8 +77,6 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
  * Free the PID object
  */
 PIDController::~PIDController() {
-  takeMutex(m_semaphore);
-  deleteMutex(m_semaphore);
   delete m_controlLoop;
 }
 
@@ -110,20 +105,20 @@ void PIDController::Calculate() {
   PIDSource *pidInput;
   PIDOutput *pidOutput;
 
-  CRITICAL_REGION(m_semaphore) {
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
     pidInput = m_pidInput;
     pidOutput = m_pidOutput;
     enabled = m_enabled;
     pidInput = m_pidInput;
   }
-  END_REGION;
 
   if (pidInput == nullptr) return;
   if (pidOutput == nullptr) return;
 
   if (enabled) {
     {
-      Synchronized sync(m_semaphore);
+      std::unique_lock<priority_mutex> sync(m_mutex);
       float input = pidInput->PIDGet();
       float result;
       PIDOutput *pidOutput;
@@ -176,12 +171,12 @@ void PIDController::Calculate() {
  * @param d Differential coefficient
  */
 void PIDController::SetPID(double p, double i, double d) {
-  CRITICAL_REGION(m_semaphore) {
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
     m_P = p;
     m_I = i;
     m_D = d;
   }
-  END_REGION;
 
   if (m_table != nullptr) {
     m_table->PutNumber("p", m_P);
@@ -199,13 +194,13 @@ void PIDController::SetPID(double p, double i, double d) {
  * @param f Feed forward coefficient
  */
 void PIDController::SetPID(double p, double i, double d, double f) {
-  CRITICAL_REGION(m_semaphore) {
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
     m_P = p;
     m_I = i;
     m_D = d;
     m_F = f;
   }
-  END_REGION;
 
   if (m_table != nullptr) {
     m_table->PutNumber("p", m_P);
@@ -220,8 +215,8 @@ void PIDController::SetPID(double p, double i, double d, double f) {
  * @return proportional coefficient
  */
 double PIDController::GetP() const {
-  CRITICAL_REGION(m_semaphore) { return m_P; }
-  END_REGION;
+  std::unique_lock<priority_mutex> sync(m_mutex);
+  return m_P;
 }
 
 /**
@@ -229,8 +224,8 @@ double PIDController::GetP() const {
  * @return integral coefficient
  */
 double PIDController::GetI() const {
-  CRITICAL_REGION(m_semaphore) { return m_I; }
-  END_REGION;
+  std::unique_lock<priority_mutex> sync(m_mutex);
+  return m_I;
 }
 
 /**
@@ -238,8 +233,8 @@ double PIDController::GetI() const {
  * @return differential coefficient
  */
 double PIDController::GetD() const {
-  CRITICAL_REGION(m_semaphore) { return m_D; }
-  END_REGION;
+  std::unique_lock<priority_mutex> sync(m_mutex);
+  return m_D;
 }
 
 /**
@@ -247,8 +242,8 @@ double PIDController::GetD() const {
  * @return Feed forward coefficient
  */
 double PIDController::GetF() const {
-  CRITICAL_REGION(m_semaphore) { return m_F; }
-  END_REGION;
+  std::unique_lock<priority_mutex> sync(m_mutex);
+  return m_F;
 }
 
 /**
@@ -257,10 +252,8 @@ double PIDController::GetF() const {
  * @return the latest calculated output
  */
 float PIDController::Get() const {
-  float result;
-  CRITICAL_REGION(m_semaphore) { result = m_result; }
-  END_REGION;
-  return result;
+  std::unique_lock<priority_mutex> sync(m_mutex);
+  return m_result;
 }
 
 /**
@@ -271,8 +264,8 @@ float PIDController::Get() const {
  * @param continuous Set to true turns on continuous, false turns off continuous
  */
 void PIDController::SetContinuous(bool continuous) {
-  CRITICAL_REGION(m_semaphore) { m_continuous = continuous; }
-  END_REGION;
+  std::unique_lock<priority_mutex> sync(m_mutex);
+  m_continuous = continuous;
 }
 
 /**
@@ -282,11 +275,11 @@ void PIDController::SetContinuous(bool continuous) {
  * @param maximumInput the maximum value expected from the output
  */
 void PIDController::SetInputRange(float minimumInput, float maximumInput) {
-  CRITICAL_REGION(m_semaphore) {
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
     m_minimumInput = minimumInput;
     m_maximumInput = maximumInput;
   }
-  END_REGION;
 
   SetSetpoint(m_setpoint);
 }
@@ -298,11 +291,11 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput) {
  * @param maximumOutput the maximum value to write to the output
  */
 void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
-  CRITICAL_REGION(m_semaphore) {
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
     m_minimumOutput = minimumOutput;
     m_maximumOutput = maximumOutput;
   }
-  END_REGION;
 }
 
 /**
@@ -310,7 +303,8 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
  * @param setpoint the desired setpoint
  */
 void PIDController::SetSetpoint(float setpoint) {
-  CRITICAL_REGION(m_semaphore) {
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
     if (m_maximumInput > m_minimumInput) {
       if (setpoint > m_maximumInput)
         m_setpoint = m_maximumInput;
@@ -322,7 +316,6 @@ void PIDController::SetSetpoint(float setpoint) {
       m_setpoint = setpoint;
     }
   }
-  END_REGION;
 
   if (m_table != nullptr) {
     m_table->PutNumber("setpoint", m_setpoint);
@@ -334,10 +327,8 @@ void PIDController::SetSetpoint(float setpoint) {
  * @return the current setpoint
  */
 double PIDController::GetSetpoint() const {
-  float setpoint;
-  CRITICAL_REGION(m_semaphore) { setpoint = m_setpoint; }
-  END_REGION;
-  return setpoint;
+  std::unique_lock<priority_mutex> sync(m_mutex);
+  return m_setpoint;
 }
 
 /**
@@ -345,12 +336,12 @@ double PIDController::GetSetpoint() const {
  * @return the current error
  */
 float PIDController::GetError() const {
-  float error;
   double pidInput;
-  CRITICAL_REGION(m_semaphore) { pidInput = m_pidInput->PIDGet(); }
-  END_REGION;
-  error = GetSetpoint() - pidInput;
-  return error;
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
+    pidInput = m_pidInput->PIDGet();
+  }
+  return GetSetpoint() - pidInput;
 }
 
 /*
@@ -359,11 +350,11 @@ float PIDController::GetError() const {
  * @param percentage error which is tolerable
  */
 void PIDController::SetTolerance(float percent) {
-  CRITICAL_REGION(m_semaphore) {
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
     m_toleranceType = kPercentTolerance;
     m_tolerance = percent;
   }
-  END_REGION;
 }
 
 /*
@@ -372,11 +363,11 @@ void PIDController::SetTolerance(float percent) {
  * @param percentage error which is tolerable
  */
 void PIDController::SetPercentTolerance(float percent) {
-  CRITICAL_REGION(m_semaphore) {
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
     m_toleranceType = kPercentTolerance;
     m_tolerance = percent;
   }
-  END_REGION;
 }
 
 /*
@@ -385,11 +376,11 @@ void PIDController::SetPercentTolerance(float percent) {
  * @param percentage error which is tolerable
  */
 void PIDController::SetAbsoluteTolerance(float absTolerance) {
-  CRITICAL_REGION(m_semaphore) {
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
     m_toleranceType = kAbsoluteTolerance;
     m_tolerance = absTolerance;
   }
-  END_REGION;
 }
 
 /*
@@ -402,32 +393,31 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) {
  * time.
  */
 bool PIDController::OnTarget() const {
-  bool temp;
   double error = GetError();
-  CRITICAL_REGION(m_semaphore) {
-    switch (m_toleranceType) {
-      case kPercentTolerance:
-        temp = fabs(error) <
-               (m_tolerance / 100 * (m_maximumInput - m_minimumInput));
-        break;
-      case kAbsoluteTolerance:
-        temp = fabs(error) < m_tolerance;
-        break;
+
+  std::unique_lock<priority_mutex> sync(m_mutex);
+  switch (m_toleranceType) {
+    case kPercentTolerance:
+      return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
+      break;
+    case kAbsoluteTolerance:
+      return fabs(error) < m_tolerance;
+      break;
+    case kNoTolerance:
       // TODO: this case needs an error
-      case kNoTolerance:
-        temp = false;
-    }
+      return false;
   }
-  END_REGION;
-  return temp;
+  return false;
 }
 
 /**
  * Begin running the PIDController
  */
 void PIDController::Enable() {
-  CRITICAL_REGION(m_semaphore) { m_enabled = true; }
-  END_REGION;
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
+    m_enabled = true;
+  }
 
   if (m_table != nullptr) {
     m_table->PutBoolean("enabled", true);
@@ -438,11 +428,11 @@ void PIDController::Enable() {
  * Stop running the PIDController, this sets the output to zero before stopping.
  */
 void PIDController::Disable() {
-  CRITICAL_REGION(m_semaphore) {
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
     m_pidOutput->PIDWrite(0);
     m_enabled = false;
   }
-  END_REGION;
 
   if (m_table != nullptr) {
     m_table->PutBoolean("enabled", false);
@@ -453,10 +443,8 @@ void PIDController::Disable() {
  * Return true if PIDController is enabled.
  */
 bool PIDController::IsEnabled() const {
-  bool enabled;
-  CRITICAL_REGION(m_semaphore) { enabled = m_enabled; }
-  END_REGION;
-  return enabled;
+  std::unique_lock<priority_mutex> sync(m_mutex);
+  return m_enabled;
 }
 
 /**
@@ -465,12 +453,10 @@ bool PIDController::IsEnabled() const {
 void PIDController::Reset() {
   Disable();
 
-  CRITICAL_REGION(m_semaphore) {
-    m_prevError = 0;
-    m_totalError = 0;
-    m_result = 0;
-  }
-  END_REGION;
+  std::unique_lock<priority_mutex> sync(m_mutex);
+  m_prevError = 0;
+  m_totalError = 0;
+  m_result = 0;
 }
 
 std::string PIDController::GetSmartDashboardType() const {
diff --git a/wpilibc/wpilibC++Devices/src/Preferences.cpp b/wpilibc/wpilibC++Devices/src/Preferences.cpp
index 75754eb873..9284bb56a6 100644
--- a/wpilibc/wpilibC++Devices/src/Preferences.cpp
+++ b/wpilibc/wpilibC++Devices/src/Preferences.cpp
@@ -7,7 +7,6 @@
 #include "Preferences.h"
 
 //#include "NetworkCommunication/UsageReporting.h"
-#include "HAL/cpp/Synchronized.hpp"
 #include "WPIErrors.h"
 #include "HAL/HAL.hpp"
 
@@ -28,25 +27,18 @@ static const char *kValueSuffix = "\"\n";
 Preferences::Preferences()
     : m_readTask("PreferencesReadTask", (FUNCPTR)Preferences::InitReadTask),
       m_writeTask("PreferencesWriteTask", (FUNCPTR)Preferences::InitWriteTask) {
-  m_fileLock = initializeMutexRecursive();
-  m_fileOpStarted = initializeSemaphore(SEMAPHORE_EMPTY);
-  m_tableLock = initializeMutexRecursive();
-
-  Synchronized sync(m_fileLock);
+  std::unique_lock<priority_recursive_mutex> sync(m_fileLock);
   m_readTask.Start((uint32_t) this);
-  takeSemaphore(m_fileOpStarted);
+
+  /* The main thread initially blocks on the semaphore. The read task signals
+   * the main thread to continue after it has locked the table mutex (so the
+   * table will be fully populated when the main thread can finally access it).
+   */
+  m_fileOpStarted.take();
 
   HALReport(HALUsageReporting::kResourceType_Preferences, 0);
 }
 
-Preferences::~Preferences() {
-  takeMutex(m_tableLock);
-  deleteMutex(m_tableLock);
-  takeMutex(m_fileLock);
-  deleteSemaphore(m_fileOpStarted);
-  deleteMutex(m_fileLock);
-}
-
 /**
  * Get the one and only {@link Preferences} object
  * @return pointer to the {@link Preferences}
@@ -300,9 +292,9 @@ void Preferences::PutLong(const char *key, int64_t value) {
  * saved before continuing.</p>
  */
 void Preferences::Save() {
-  Synchronized sync(m_fileLock);
+  std::unique_lock<priority_recursive_mutex> sync(m_fileLock);
   m_writeTask.Start((uint32_t) this);
-  takeSemaphore(m_fileOpStarted);
+  m_fileOpStarted.take();
 }
 
 /**
@@ -333,7 +325,7 @@ void Preferences::Remove(const char *key) {
  * @return the value (or empty if none exists)
  */
 std::string Preferences::Get(const char *key) {
-  Synchronized sync(m_tableLock);
+  std::unique_lock<priority_recursive_mutex> sync(m_tableLock);
   if (key == nullptr) {
     wpi_setWPIErrorWithContext(NullParameter, "key");
     return "";
@@ -347,7 +339,7 @@ std::string Preferences::Get(const char *key) {
  * @param value the value
  */
 void Preferences::Put(const char *key, std::string value) {
-  Synchronized sync(m_tableLock);
+  std::unique_lock<priority_recursive_mutex> sync(m_tableLock);
   if (key == nullptr) {
     wpi_setWPIErrorWithContext(NullParameter, "key");
     return;
@@ -375,8 +367,8 @@ void Preferences::Put(const char *key, std::string value) {
  * first created.
  */
 void Preferences::ReadTaskRun() {
-  Synchronized sync(m_tableLock);
-  giveSemaphore(m_fileOpStarted);
+  std::unique_lock<priority_recursive_mutex> sync(m_tableLock);
+  m_fileOpStarted.give();
 
   std::string comment;
 
@@ -478,8 +470,8 @@ void Preferences::ReadTaskRun() {
  * called.
  */
 void Preferences::WriteTaskRun() {
-  Synchronized sync(m_tableLock);
-  giveSemaphore(m_fileOpStarted);
+  std::unique_lock<priority_recursive_mutex> sync(m_tableLock);
+  m_fileOpStarted.give();
 
   FILE *file = nullptr;
   file = fopen(kFileName, "w");
@@ -521,12 +513,13 @@ static bool isKeyAcceptable(const std::string &value) {
   }
   return true;
 }
+
 void Preferences::ValueChanged(ITable *table, const std::string &key,
                                EntryValue value, bool isNew) {
   if (key == kSaveField) {
     if (table->GetBoolean(kSaveField, false)) Save();
   } else {
-    Synchronized sync(m_tableLock);
+    std::unique_lock<priority_recursive_mutex> sync(m_tableLock);
 
     if (!isKeyAcceptable(key) ||
         table->GetString(key, "").find('"') != std::string::npos) {
diff --git a/wpilibc/wpilibC++Devices/src/Task.cpp b/wpilibc/wpilibC++Devices/src/Task.cpp
index 8c27151609..2c8bf2d2f9 100644
--- a/wpilibc/wpilibC++Devices/src/Task.cpp
+++ b/wpilibc/wpilibC++Devices/src/Task.cpp
@@ -14,6 +14,13 @@
 #include <stdio.h>
 #include "HAL/HAL.hpp"
 
+#ifndef OK
+#define OK 0
+#endif /* OK */
+#ifndef ERROR
+#define ERROR (-1)
+#endif /* ERROR */
+
 const uint32_t Task::kDefaultPriority;
 
 /**
diff --git a/wpilibc/wpilibC++Devices/src/Timer.cpp b/wpilibc/wpilibC++Devices/src/Timer.cpp
index acb794f4a4..ea11d26bbe 100644
--- a/wpilibc/wpilibC++Devices/src/Timer.cpp
+++ b/wpilibc/wpilibC++Devices/src/Timer.cpp
@@ -10,7 +10,6 @@
 #include <time.h>
 
 #include "HAL/HAL.hpp"
-#include "HAL/cpp/Synchronized.hpp"
 #include "Utility.h"
 #include <iostream>
 
@@ -62,12 +61,9 @@ double GetTime() {
 Timer::Timer() {
   // Creates a semaphore to control access to critical regions.
   // Initially 'open'
-  m_semaphore = initializeMutexNormal();
   Reset();
 }
 
-Timer::~Timer() { deleteMutex(m_semaphore); }
-
 /**
  * Get the current time from the timer. If the clock is running it is derived
  * from
@@ -81,7 +77,7 @@ double Timer::Get() const {
   double result;
   double currentTime = GetFPGATimestamp();
 
-  Synchronized sync(m_semaphore);
+  std::unique_lock<priority_mutex> sync(m_mutex);
   if (m_running) {
     // If the current time is before the start time, then the FPGA clock
     // rolled over.  Compensate by adding the ~71 minutes that it takes
@@ -105,7 +101,7 @@ double Timer::Get() const {
  * now
  */
 void Timer::Reset() {
-  Synchronized sync(m_semaphore);
+  std::unique_lock<priority_mutex> sync(m_mutex);
   m_accumulatedTime = 0;
   m_startTime = GetFPGATimestamp();
 }
@@ -116,7 +112,7 @@ void Timer::Reset() {
  * relative to the system clock.
  */
 void Timer::Start() {
-  Synchronized sync(m_semaphore);
+  std::unique_lock<priority_mutex> sync(m_mutex);
   if (!m_running) {
     m_startTime = GetFPGATimestamp();
     m_running = true;
@@ -132,7 +128,7 @@ void Timer::Start() {
 void Timer::Stop() {
   double temp = Get();
 
-  Synchronized sync(m_semaphore);
+  std::unique_lock<priority_mutex> sync(m_mutex);
   if (m_running) {
     m_accumulatedTime = temp;
     m_running = false;
@@ -149,7 +145,7 @@ void Timer::Stop() {
  */
 bool Timer::HasPeriodPassed(double period) {
   if (Get() > period) {
-    Synchronized sync(m_semaphore);
+    std::unique_lock<priority_mutex> sync(m_mutex);
     // Advance the start time by the period.
     m_startTime += period;
     // Don't set it to the current time... we want to avoid drift.
diff --git a/wpilibc/wpilibC++Devices/src/USBCamera.cpp b/wpilibc/wpilibC++Devices/src/USBCamera.cpp
index b6f1bd7f25..74870ad50d 100644
--- a/wpilibc/wpilibC++Devices/src/USBCamera.cpp
+++ b/wpilibc/wpilibC++Devices/src/USBCamera.cpp
@@ -76,7 +76,7 @@ USBCamera::USBCamera(std::string name, bool useJpeg)
       m_useJpeg(useJpeg) {}
 
 void USBCamera::OpenCamera() {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   for (unsigned int i = 0; i < 3; i++) {
     uInt32 id = 0;
     // Can't use SAFE_IMAQ_CALL here because we only error on the third time
@@ -96,7 +96,7 @@ void USBCamera::OpenCamera() {
 }
 
 void USBCamera::CloseCamera() {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   if (!m_open) return;
   SAFE_IMAQ_CALL(IMAQdxCloseCamera, m_id);
   m_id = 0;
@@ -104,7 +104,7 @@ void USBCamera::CloseCamera() {
 }
 
 void USBCamera::StartCapture() {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   if (!m_open || m_active) return;
   SAFE_IMAQ_CALL(IMAQdxConfigureGrab, m_id);
   SAFE_IMAQ_CALL(IMAQdxStartAcquisition, m_id);
@@ -112,7 +112,7 @@ void USBCamera::StartCapture() {
 }
 
 void USBCamera::StopCapture() {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   if (!m_open || !m_active) return;
   SAFE_IMAQ_CALL(IMAQdxStopAcquisition, m_id);
   SAFE_IMAQ_CALL(IMAQdxUnconfigureAcquisition, m_id);
@@ -120,7 +120,7 @@ void USBCamera::StopCapture() {
 }
 
 void USBCamera::UpdateSettings() {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   bool wasActive = m_active;
 
   if (wasActive) StopCapture();
@@ -214,7 +214,7 @@ void USBCamera::UpdateSettings() {
 }
 
 void USBCamera::SetFPS(double fps) {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   if (m_fps != fps) {
     m_needSettingsUpdate = true;
     m_fps = fps;
@@ -222,7 +222,7 @@ void USBCamera::SetFPS(double fps) {
 }
 
 void USBCamera::SetSize(unsigned int width, unsigned int height) {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   if (m_width != width || m_height != height) {
     m_needSettingsUpdate = true;
     m_width = width;
@@ -231,7 +231,7 @@ void USBCamera::SetSize(unsigned int width, unsigned int height) {
 }
 
 void USBCamera::SetBrightness(unsigned int brightness) {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   if (m_brightness != brightness) {
     m_needSettingsUpdate = true;
     m_brightness = brightness;
@@ -239,12 +239,12 @@ void USBCamera::SetBrightness(unsigned int brightness) {
 }
 
 unsigned int USBCamera::GetBrightness() {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   return m_brightness;
 }
 
 void USBCamera::SetWhiteBalanceAuto() {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   m_whiteBalance = AUTO;
   m_whiteBalanceValue = 0;
   m_whiteBalanceValuePresent = false;
@@ -252,7 +252,7 @@ void USBCamera::SetWhiteBalanceAuto() {
 }
 
 void USBCamera::SetWhiteBalanceHoldCurrent() {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   m_whiteBalance = MANUAL;
   m_whiteBalanceValue = 0;
   m_whiteBalanceValuePresent = false;
@@ -260,7 +260,7 @@ void USBCamera::SetWhiteBalanceHoldCurrent() {
 }
 
 void USBCamera::SetWhiteBalanceManual(unsigned int whiteBalance) {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   m_whiteBalance = MANUAL;
   m_whiteBalanceValue = whiteBalance;
   m_whiteBalanceValuePresent = true;
@@ -268,7 +268,7 @@ void USBCamera::SetWhiteBalanceManual(unsigned int whiteBalance) {
 }
 
 void USBCamera::SetExposureAuto() {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   m_exposure = AUTO;
   m_exposureValue = 0;
   m_exposureValuePresent = false;
@@ -276,7 +276,7 @@ void USBCamera::SetExposureAuto() {
 }
 
 void USBCamera::SetExposureHoldCurrent() {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   m_exposure = MANUAL;
   m_exposureValue = 0;
   m_exposureValuePresent = false;
@@ -284,7 +284,7 @@ void USBCamera::SetExposureHoldCurrent() {
 }
 
 void USBCamera::SetExposureManual(unsigned int level) {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   m_exposure = MANUAL;
   if (level > 100)
     m_exposureValue = 100;
@@ -295,7 +295,7 @@ void USBCamera::SetExposureManual(unsigned int level) {
 }
 
 void USBCamera::GetImage(Image* image) {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   if (m_needSettingsUpdate || m_useJpeg) {
     m_needSettingsUpdate = false;
     m_useJpeg = false;
@@ -308,7 +308,7 @@ void USBCamera::GetImage(Image* image) {
 }
 
 unsigned int USBCamera::GetImageData(void* buffer, unsigned int bufferSize) {
-  std::unique_lock<std::recursive_mutex> lock(m_mutex);
+  std::unique_lock<priority_recursive_mutex> lock(m_mutex);
   if (m_needSettingsUpdate || !m_useJpeg) {
     m_needSettingsUpdate = false;
     m_useJpeg = true;
diff --git a/wpilibc/wpilibC++Devices/src/Ultrasonic.cpp b/wpilibc/wpilibC++Devices/src/Ultrasonic.cpp
index 9f5fe77934..ca260b9f51 100644
--- a/wpilibc/wpilibC++Devices/src/Ultrasonic.cpp
+++ b/wpilibc/wpilibC++Devices/src/Ultrasonic.cpp
@@ -30,7 +30,7 @@ Task Ultrasonic::m_task(
 Ultrasonic *Ultrasonic::m_firstSensor =
     nullptr;  // head of the ultrasonic sensor list
 bool Ultrasonic::m_automaticEnabled = false;  // automatic round robin mode
-SEMAPHORE_ID Ultrasonic::m_semaphore = nullptr;
+priority_mutex Ultrasonic::m_mutex;
 
 /**
  * Background task that goes through the list of ultrasonic sensors and pings
@@ -67,14 +67,13 @@ void Ultrasonic::UltrasonicChecker() {
  */
 void Ultrasonic::Initialize() {
   bool originalMode = m_automaticEnabled;
-  if (m_semaphore == nullptr) m_semaphore = initializeSemaphore(SEMAPHORE_FULL);
   SetAutomaticMode(false);     // kill task when adding a new sensor
-  takeSemaphore(m_semaphore);  // link this instance on the list
+  // link this instance on the list
   {
+    std::unique_lock<priority_mutex> lock(m_mutex);
     m_nextSensor = m_firstSensor;
     m_firstSensor = this;
   }
-  giveSemaphore(m_semaphore);
 
   m_counter = new Counter(m_echoChannel);  // set up counter for this sensor
   m_counter->SetMaxPeriod(1.0);
@@ -170,8 +169,8 @@ Ultrasonic::~Ultrasonic() {
   }
   wpi_assert(m_firstSensor != nullptr);
 
-  takeSemaphore(m_semaphore);
   {
+    std::unique_lock<priority_mutex> lock(m_mutex);
     if (this == m_firstSensor) {
       m_firstSensor = m_nextSensor;
       if (m_firstSensor == nullptr) {
@@ -187,7 +186,6 @@ Ultrasonic::~Ultrasonic() {
       }
     }
   }
-  giveSemaphore(m_semaphore);
   if (m_firstSensor != nullptr && wasAutomaticMode) SetAutomaticMode(true);
 }
 
diff --git a/wpilibc/wpilibC++Devices/src/Vision/AxisCamera.cpp b/wpilibc/wpilibC++Devices/src/Vision/AxisCamera.cpp
index 67d39d7707..38552c9945 100644
--- a/wpilibc/wpilibC++Devices/src/Vision/AxisCamera.cpp
+++ b/wpilibc/wpilibC++Devices/src/Vision/AxisCamera.cpp
@@ -70,7 +70,7 @@ int AxisCamera::GetImage(Image *image) {
     return 0;
   }
 
-  std::lock_guard<std::mutex> lock(m_imageDataMutex);
+  std::lock_guard<priority_mutex> lock(m_imageDataMutex);
 
   Priv_ReadJPEGString_C(image, m_imageData.data(), m_imageData.size());
 
@@ -116,7 +116,7 @@ HSLImage *AxisCamera::GetImage() {
  */
 int AxisCamera::CopyJPEG(char **destImage, unsigned int &destImageSize,
                          unsigned int &destImageBufferSize) {
-  std::lock_guard<std::mutex> lock(m_imageDataMutex);
+  std::lock_guard<priority_mutex> lock(m_imageDataMutex);
   if (destImage == nullptr)
     wpi_setWPIErrorWithContext(NullParameter, "destImage must not be nullptr");
 
@@ -152,7 +152,7 @@ void AxisCamera::WriteBrightness(int brightness) {
     return;
   }
 
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
 
   if (m_brightness != brightness) {
     m_brightness = brightness;
@@ -164,7 +164,7 @@ void AxisCamera::WriteBrightness(int brightness) {
  * @return The configured brightness of the camera images
  */
 int AxisCamera::GetBrightness() {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
   return m_brightness;
 }
 
@@ -173,7 +173,7 @@ int AxisCamera::GetBrightness() {
  * @param whiteBalance Valid values from the <code>WhiteBalance</code> enum.
  */
 void AxisCamera::WriteWhiteBalance(AxisCamera::WhiteBalance whiteBalance) {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
 
   if (m_whiteBalance != whiteBalance) {
     m_whiteBalance = whiteBalance;
@@ -185,7 +185,7 @@ void AxisCamera::WriteWhiteBalance(AxisCamera::WhiteBalance whiteBalance) {
  * @return The configured white balances of the camera images
  */
 AxisCamera::WhiteBalance AxisCamera::GetWhiteBalance() {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
   return m_whiteBalance;
 }
 
@@ -200,7 +200,7 @@ void AxisCamera::WriteColorLevel(int colorLevel) {
     return;
   }
 
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
 
   if (m_colorLevel != colorLevel) {
     m_colorLevel = colorLevel;
@@ -212,7 +212,7 @@ void AxisCamera::WriteColorLevel(int colorLevel) {
  * @return The configured color level of the camera images
  */
 int AxisCamera::GetColorLevel() {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
   return m_colorLevel;
 }
 
@@ -222,7 +222,7 @@ int AxisCamera::GetColorLevel() {
  */
 void AxisCamera::WriteExposureControl(
     AxisCamera::ExposureControl exposureControl) {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
 
   if (m_exposureControl != exposureControl) {
     m_exposureControl = exposureControl;
@@ -234,7 +234,7 @@ void AxisCamera::WriteExposureControl(
  * @return The configured exposure control mode of the camera
  */
 AxisCamera::ExposureControl AxisCamera::GetExposureControl() {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
   return m_exposureControl;
 }
 
@@ -253,7 +253,7 @@ void AxisCamera::WriteExposurePriority(int exposurePriority) {
     return;
   }
 
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
 
   if (m_exposurePriority != exposurePriority) {
     m_exposurePriority = exposurePriority;
@@ -265,7 +265,7 @@ void AxisCamera::WriteExposurePriority(int exposurePriority) {
  * @return The configured exposure priority of the camera
  */
 int AxisCamera::GetExposurePriority() {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
   return m_exposurePriority;
 }
 
@@ -276,7 +276,7 @@ int AxisCamera::GetExposurePriority() {
  * exposure permitting.
  */
 void AxisCamera::WriteMaxFPS(int maxFPS) {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
 
   if (m_maxFPS != maxFPS) {
     m_maxFPS = maxFPS;
@@ -289,7 +289,7 @@ void AxisCamera::WriteMaxFPS(int maxFPS) {
  * @return The configured maximum FPS of the camera
  */
 int AxisCamera::GetMaxFPS() {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
   return m_maxFPS;
 }
 
@@ -298,7 +298,7 @@ int AxisCamera::GetMaxFPS() {
  * @param resolution The camera resolution value to write to the camera.
  */
 void AxisCamera::WriteResolution(AxisCamera::Resolution resolution) {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
 
   if (m_resolution != resolution) {
     m_resolution = resolution;
@@ -312,7 +312,7 @@ void AxisCamera::WriteResolution(AxisCamera::Resolution resolution) {
  * resolution as the most recent image, if it was changed recently.)
  */
 AxisCamera::Resolution AxisCamera::GetResolution() {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
   return m_resolution;
 }
 
@@ -324,7 +324,7 @@ AxisCamera::Resolution AxisCamera::GetResolution() {
  * or <code>AxisCamera::Rotation::k180</code>)
  */
 void AxisCamera::WriteRotation(AxisCamera::Rotation rotation) {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
 
   if (m_rotation != rotation) {
     m_rotation = rotation;
@@ -337,7 +337,7 @@ void AxisCamera::WriteRotation(AxisCamera::Rotation rotation) {
  * @return The configured rotation mode of the camera
  */
 AxisCamera::Rotation AxisCamera::GetRotation() {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
   return m_rotation;
 }
 
@@ -352,7 +352,7 @@ void AxisCamera::WriteCompression(int compression) {
     return;
   }
 
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
 
   if (m_compression != compression) {
     m_compression = compression;
@@ -365,7 +365,7 @@ void AxisCamera::WriteCompression(int compression) {
  * @return The configured compression level of the camera
  */
 int AxisCamera::GetCompression() {
-  std::lock_guard<std::mutex> lock(m_parametersMutex);
+  std::lock_guard<priority_mutex> lock(m_parametersMutex);
   return m_compression;
 }
 
@@ -468,7 +468,7 @@ void AxisCamera::ReadImagesFromCamera() {
 
     // Update image
     {
-      std::lock_guard<std::mutex> lock(m_imageDataMutex);
+      std::lock_guard<priority_mutex> lock(m_imageDataMutex);
 
       m_imageData.assign(imgBuffer, imgBuffer + imgBufferLength);
       m_freshImage = true;
diff --git a/wpilibc/wpilibC++IntegrationTests/src/ConditionVariableTest.cpp b/wpilibc/wpilibC++IntegrationTests/src/ConditionVariableTest.cpp
new file mode 100644
index 0000000000..09e2f9a500
--- /dev/null
+++ b/wpilibc/wpilibC++IntegrationTests/src/ConditionVariableTest.cpp
@@ -0,0 +1,294 @@
+#include "WPILib.h"
+#include "TestBench.h"
+
+#include "HAL/cpp/priority_condition_variable.h"
+#include "HAL/cpp/priority_mutex.h"
+
+#include "gtest/gtest.h"
+#include <condition_variable>
+#include <mutex>
+#include <thread>
+#include <chrono>
+
+namespace wpilib {
+namespace testing {
+
+// Tests that the condition variable class which we wrote ourselves actually
+// does work.
+class ConditionVariableTest : public ::testing::Test {
+ protected:
+  typedef std::unique_lock<priority_mutex> priority_lock;
+
+  // Condition variable to test.
+  priority_condition_variable m_cond;
+
+  // Mutex to pass to condition variable when waiting.
+  priority_mutex m_mutex;
+
+  // flags for testing when threads are completed.
+  std::atomic<bool> m_done1{false}, m_done2{false};
+  // Threads to use for testing. We want multiple threads to ensure that it
+  // behaves correctly when multiple processes are waiting on a signal.
+  std::thread m_watcher1, m_watcher2;
+
+  // Information for when running with predicates.
+  std::atomic<bool> m_pred_var{false};
+
+  void ShortSleep(unsigned long time=10) {
+    std::this_thread::sleep_for(std::chrono::milliseconds(time));
+  }
+
+  // Start up the given threads with a wait function. The wait function should
+  // call some version of m_cond.wait and should take as an argument a reference
+  // to an std::atomic<bool> which it will set to true when it is ready to have
+  // join called on it.
+  template <class Function>
+  void StartThreads(Function wait) {
+    m_watcher1 = std::thread(wait, std::ref(m_done1));
+    m_watcher2 = std::thread(wait, std::ref(m_done2));
+
+    // Wait briefly to let the lock be unlocked.
+    ShortSleep();
+    bool locked = m_mutex.try_lock();
+    if (locked) m_mutex.unlock();
+    EXPECT_TRUE(locked)
+        << "The condition variable failed to unlock the lock.";
+  }
+
+  void NotifyAll() { m_cond.notify_all(); }
+  void NotifyOne() { m_cond.notify_one(); }
+
+  // Test that all the threads are notified by a notify_all() call.
+  void NotifyAllTest() {
+    NotifyAll();
+    // Wait briefly to let the lock be re-locked.
+    ShortSleep();
+    EXPECT_TRUE(m_done1) << "watcher1 failed to be notified.";
+    EXPECT_TRUE(m_done2) << "watcher2 failed to be notified.";
+  }
+
+  // For use when testing predicates. First tries signalling the threads with
+  // the predicate set to false (and ensures that they do not activate) and then
+  // tests with the predicate set to true.
+  void PredicateTest() {
+    m_pred_var = false;
+    NotifyAll();
+    ShortSleep();
+    EXPECT_FALSE(m_done1) << "watcher1 didn't pay attention to its predicate.";
+    EXPECT_FALSE(m_done2) << "watcher2 didn't pay attention to its predicate.";
+    m_pred_var = true;
+    NotifyAllTest();
+  }
+
+  // Used by the WaitFor and WaitUntil tests to test that, without a predicate,
+  // the timeout works properly.
+  void WaitTimeTest(bool wait_for) {
+    std::atomic<bool> timed_out{true};
+    auto wait_until = [this, &timed_out, wait_for](std::atomic<bool> &done) {
+      priority_lock lock(m_mutex);
+      done = false;
+      if (wait_for) {
+        auto wait_time = std::chrono::milliseconds(100);
+        timed_out =
+            m_cond.wait_for(lock, wait_time) == std::cv_status::timeout;
+      } else {
+        auto wait_time =
+            std::chrono::system_clock::now() + std::chrono::milliseconds(100);
+        timed_out =
+            m_cond.wait_until(lock, wait_time) == std::cv_status::timeout;
+      }
+      EXPECT_TRUE(lock.owns_lock())
+          << "The condition variable should have reacquired the lock.";
+      done = true;
+    };
+
+    // First, test without timing out.
+    timed_out = true;
+    StartThreads(wait_until);
+
+    NotifyAllTest();
+    EXPECT_FALSE(timed_out) << "The watcher should not have timed out.";
+
+    TearDown();
+
+    // Next, test and time out.
+    timed_out = false;
+    StartThreads(wait_until);
+
+    ShortSleep(110);
+
+    EXPECT_TRUE(m_done1) << "watcher1 should have timed out.";
+    EXPECT_TRUE(m_done2) << "watcher2 should have timed out.";
+    EXPECT_TRUE(timed_out) << "The watcher should have timed out.";
+  }
+
+  // For use with tests that have a timeout and a predicate.
+  void WaitTimePredicateTest(bool wait_for) {
+    // The condition_variable return value from the wait_for or wait_until
+    // function should in the case of having a predicate, by a boolean. If the
+    // predicate is true, then the return value will always be true. If the
+    // condition times out and, at the time of the timeout, the predicate is
+    // false, the return value will be false.
+    std::atomic<bool> retval{true};
+    auto predicate = [this]() -> bool { return m_pred_var; };
+    auto wait_until =
+        [this, &retval, predicate, wait_for](std::atomic<bool> &done) {
+      priority_lock lock(m_mutex);
+      done = false;
+      if (wait_for) {
+        auto wait_time = std::chrono::milliseconds(100);
+        retval = m_cond.wait_for(lock, wait_time, predicate);
+      } else {
+        auto wait_time =
+            std::chrono::system_clock::now() + std::chrono::milliseconds(100);
+        retval = m_cond.wait_until(lock, wait_time, predicate);
+      }
+      EXPECT_TRUE(lock.owns_lock())
+          << "The condition variable should have reacquired the lock.";
+      done = true;
+    };
+
+    // Test without timing out and with the predicate set to true.
+    retval = true;
+    m_pred_var = true;
+    StartThreads(wait_until);
+
+    NotifyAllTest();
+    EXPECT_TRUE(retval) << "The watcher should not have timed out.";
+
+    TearDown();
+
+    // Test with timing out and with the predicate set to true.
+    retval = false;
+    m_pred_var = false;
+    StartThreads(wait_until);
+
+    ShortSleep(110);
+
+    EXPECT_TRUE(m_done1) << "watcher1 should have finished.";
+    EXPECT_TRUE(m_done2) << "watcher2 should have finished.";
+    EXPECT_FALSE(retval) << "The watcher should have timed out.";
+
+    TearDown();
+
+    // Test without timing out and run the PredicateTest().
+    retval = false;
+    StartThreads(wait_until);
+
+    PredicateTest();
+    EXPECT_TRUE(retval) << "The return value should have been true.";
+
+    TearDown();
+
+    // Test with timing out and the predicate set to true while we are waiting
+    // for the condition variable to time out.
+    retval = true;
+    StartThreads(wait_until);
+    ShortSleep();
+    m_pred_var = true;
+    ShortSleep(110);
+    EXPECT_TRUE(retval) << "The return value should have been true.";
+  }
+
+  virtual void TearDown() {
+    // If a thread has not completed, then continuing will cause the tests to
+    // hang forever and could cause issues. If we don't call detach, then
+    // std::terminate is called and all threads are terminated.
+    // Detaching is non-optimal, but should allow the rest of the tests to run
+    // before anything drastic occurs.
+    if (m_done1) m_watcher1.join();
+    else m_watcher1.detach();
+    if (m_done2) m_watcher2.join();
+    else m_watcher2.detach();
+  }
+};
+
+TEST_F(ConditionVariableTest, NotifyAll) {
+  auto wait = [this](std::atomic<bool> &done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait);
+
+  NotifyAllTest();
+}
+
+TEST_F(ConditionVariableTest, NotifyOne) {
+  auto wait = [this](std::atomic<bool> &done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait);
+
+  NotifyOne();
+  // Wait briefly to let things settle.
+  ShortSleep();
+  EXPECT_TRUE(m_done1 ^ m_done2) << "Only one thread should've been notified.";
+  NotifyOne();
+  ShortSleep();
+  EXPECT_TRUE(m_done2 && m_done2) << "Both threads should've been notified.";
+}
+
+TEST_F(ConditionVariableTest, WaitWithPredicate) {
+  auto predicate = [this]() -> bool { return m_pred_var; };
+  auto wait_predicate = [this, predicate](std::atomic<bool> &done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock, predicate);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait_predicate);
+
+  PredicateTest();
+}
+
+TEST_F(ConditionVariableTest, WaitUntil) {
+  WaitTimeTest(false);
+}
+
+TEST_F(ConditionVariableTest, WaitUntilWithPredicate) {
+  WaitTimePredicateTest(false);
+}
+
+TEST_F(ConditionVariableTest, WaitFor) {
+  WaitTimeTest(true);
+}
+
+TEST_F(ConditionVariableTest, WaitForWithPredicate) {
+  WaitTimePredicateTest(true);
+}
+
+TEST_F(ConditionVariableTest, NativeHandle) {
+  auto wait = [this](std::atomic<bool> &done) {
+    priority_lock lock(m_mutex);
+    done = false;
+    m_cond.wait(lock);
+    EXPECT_TRUE(lock.owns_lock())
+        << "The condition variable should have reacquired the lock.";
+    done = true;
+  };
+
+  StartThreads(wait);
+
+  pthread_cond_t* native_handle = m_cond.native_handle();
+  pthread_cond_broadcast(native_handle);
+  ShortSleep();
+  EXPECT_TRUE(m_done1) << "watcher1 failed to be notified.";
+  EXPECT_TRUE(m_done2) << "watcher2 failed to be notified.";
+}
+
+}  // namespace testing
+}  // namespace wpilib
diff --git a/wpilibc/wpilibC++IntegrationTests/src/MutexTest.cpp b/wpilibc/wpilibC++IntegrationTests/src/MutexTest.cpp
new file mode 100644
index 0000000000..a878aa4a71
--- /dev/null
+++ b/wpilibc/wpilibC++IntegrationTests/src/MutexTest.cpp
@@ -0,0 +1,260 @@
+#include "WPILib.h"
+#include "TestBench.h"
+
+#include "gtest/gtest.h"
+#include <atomic>
+#include <thread>
+#include "HAL/cpp/priority_mutex.h"
+#include <condition_variable>
+
+namespace wpilib {
+namespace testing {
+
+using std::chrono::system_clock;
+
+// Threading primitive used to notify many threads that a condition is now true.
+// The condition can not be cleared.
+class Notification {
+ public:
+  // Efficiently waits until the Notification has been notified once.
+  void Wait() {
+    std::unique_lock<priority_mutex> lock(m_mutex);
+    while (!m_set) {
+      m_condition.wait(lock);
+    }
+  }
+  // Sets the condition to true, and wakes all waiting threads.
+  void Notify() {
+    std::unique_lock<priority_mutex> lock(m_mutex);
+    m_set = true;
+    m_condition.notify_all();
+  }
+
+ private:
+  // priority_mutex used for the notification and to protect the bit.
+  priority_mutex m_mutex;
+  // Condition for threads to sleep on.
+  std::condition_variable_any m_condition;
+  // Bool which is true when the notification has been notified.
+  bool m_set = false;
+};
+
+void SetProcessorAffinity(int core_id) {
+  cpu_set_t cpuset;
+  CPU_ZERO(&cpuset);
+  CPU_SET(core_id, &cpuset);
+
+  pthread_t current_thread = pthread_self();
+  ASSERT_TRUE(
+      pthread_setaffinity_np(current_thread, sizeof(cpu_set_t), &cpuset) == 0);
+}
+
+void SetThreadRealtimePriorityOrDie(int priority) {
+  struct sched_param param;
+  // Set realtime priority for this thread
+  param.sched_priority = priority + sched_get_priority_min(SCHED_RR);
+  ASSERT_TRUE(pthread_setschedparam(pthread_self(), SCHED_RR, &param) == 0)
+      << ": Failed to set scheduler priority.";
+}
+
+// This thread holds the mutex and spins until signaled to release it and stop.
+template <typename MutexType>
+class LowPriorityThread {
+ public:
+  LowPriorityThread(MutexType *mutex)
+      : m_mutex(mutex), m_hold_mutex(1), m_success(0) {}
+
+  void operator()() {
+    SetProcessorAffinity(0);
+    SetThreadRealtimePriorityOrDie(20);
+    m_mutex->lock();
+    m_started.Notify();
+    while (m_hold_mutex.test_and_set()) {}
+    m_mutex->unlock();
+    m_success.store(1);
+  }
+
+  void WaitForStartup() { m_started.Wait(); }
+  void release_mutex() { m_hold_mutex.clear(); }
+  bool success() { return m_success.load(); }
+
+ private:
+  // priority_mutex to grab and release.
+  MutexType *m_mutex;
+  Notification m_started;
+  // Atomic type used to signal when the thread should unlock the mutex.
+  // Using a mutex to protect something could cause other issues, and a delay
+  // between setting and reading isn't a problem as long as the set is atomic.
+  std::atomic_flag m_hold_mutex;
+  std::atomic<int> m_success;
+};
+
+// This thread spins forever until signaled to stop.
+class BusyWaitingThread {
+ public:
+  BusyWaitingThread() : m_run(1), m_success(0) {}
+
+  void operator()() {
+    SetProcessorAffinity(0);
+    SetThreadRealtimePriorityOrDie(21);
+    system_clock::time_point start_time = system_clock::now();
+    m_started.Notify();
+    while (m_run.test_and_set()) {
+      // Have the busy waiting thread time out after a while.  If it times out,
+      // the test failed.
+      if (system_clock::now() - start_time > std::chrono::milliseconds(50)) {
+        return;
+      }
+    }
+    m_success.store(1);
+  }
+
+  void quit() { m_run.clear(); }
+  void WaitForStartup() { m_started.Wait(); }
+  bool success() { return m_success.load(); }
+
+ private:
+  // Use an atomic type to signal if the thread should be running or not.  A
+  // mutex could affect the scheduler, which isn't worth it.  A delay between
+  // setting and reading the new value is fine.
+  std::atomic_flag m_run;
+
+  Notification m_started;
+
+  std::atomic<int> m_success;
+};
+
+// This thread starts up, grabs the mutex, and then exits.
+template <typename MutexType>
+class HighPriorityThread {
+ public:
+  HighPriorityThread(MutexType *mutex) : m_mutex(mutex), m_success(0) {}
+
+  void operator()() {
+    SetProcessorAffinity(0);
+    SetThreadRealtimePriorityOrDie(22);
+    m_started.Notify();
+    m_mutex->lock();
+    m_success.store(1);
+  }
+
+  void WaitForStartup() { m_started.Wait(); }
+  bool success() { return m_success.load(); }
+
+ private:
+  Notification m_started;
+  MutexType *m_mutex;
+  std::atomic<int> m_success;
+};
+
+// Class to test a MutexType to see if it solves the priority inheritance
+// problem.
+//
+// To run the test, we need 3 threads, and then 1 thread to kick the test off.
+// The threads must all run on the same core, otherwise they wouldn't starve
+// eachother. The threads and their roles are as follows:
+//
+// Low priority thread:
+//   Holds a lock that the high priority thread needs, and releases it upon
+//   request.
+// Medium priority thread:
+//   Hogs the processor so that the low priority thread will never run if it's
+//   priority doesn't get bumped.
+// High priority thread:
+//   Starts up and then goes to grab the lock that the low priority thread has.
+//
+// Control thread:
+//   Sets the deadlock up so that it will happen 100% of the time by making sure
+//   that each thread in order gets to the point that it needs to be at to cause
+//   the deadlock.
+template <typename MutexType>
+class InversionTestRunner {
+ public:
+  void operator()() {
+    // This thread must run at the highest priority or it can't coordinate the
+    // inversion.  This means that it can't busy wait or everything could
+    // starve.
+    SetThreadRealtimePriorityOrDie(23);
+
+    MutexType m;
+
+    // Start the lowest priority thread and wait until it holds the lock.
+    LowPriorityThread<MutexType> low(&m);
+    std::thread low_thread(std::ref(low));
+    low.WaitForStartup();
+
+    // Start the busy waiting thread and let it get to the loop.
+    BusyWaitingThread busy;
+    std::thread busy_thread(std::ref(busy));
+    busy.WaitForStartup();
+
+    // Start the high priority thread and let it become blocked on the lock.
+    HighPriorityThread<MutexType> high(&m);
+    std::thread high_thread(std::ref(high));
+    high.WaitForStartup();
+    // Startup and locking the mutex in the high priority thread aren't atomic,
+    // but pretty close.  Wait a bit to let it happen.
+    std::this_thread::sleep_for(std::chrono::milliseconds(10));
+
+    // Release the mutex in the low priority thread.  If done right, everything
+    // should finish now.
+    low.release_mutex();
+
+    // Wait for everything to finish and compute success.
+    high_thread.join();
+    busy.quit();
+    busy_thread.join();
+    low_thread.join();
+    m_success = low.success() && busy.success() && high.success();
+  }
+
+  bool success() { return m_success; }
+
+ private:
+  bool m_success = false;
+};
+
+// Priority inversion test.
+TEST(MutexTest, PriorityInversionTest) {
+  InversionTestRunner<priority_mutex> runner;
+  std::thread runner_thread(std::ref(runner));
+  runner_thread.join();
+  EXPECT_TRUE(runner.success());
+}
+
+// Verify that the non-priority inversion mutex doesn't pass the test.
+TEST(MutexTest, StdMutexPriorityInversionTest) {
+  InversionTestRunner<std::mutex> runner;
+  std::thread runner_thread(std::ref(runner));
+  runner_thread.join();
+  EXPECT_FALSE(runner.success());
+}
+
+// Smoke test to make sure that mutexes lock and unlock.
+TEST(MutexTest, TryLock) {
+  priority_mutex m;
+  m.lock();
+  EXPECT_FALSE(m.try_lock());
+  m.unlock();
+  EXPECT_TRUE(m.try_lock());
+}
+
+// Priority inversion test.
+TEST(MutexTest, ReentrantPriorityInversionTest) {
+  InversionTestRunner<priority_recursive_mutex> runner;
+  std::thread runner_thread(std::ref(runner));
+  runner_thread.join();
+  EXPECT_TRUE(runner.success());
+}
+
+// Smoke test to make sure that mutexes lock and unlock.
+TEST(MutexTest, ReentrantTryLock) {
+  priority_recursive_mutex m;
+  m.lock();
+  EXPECT_TRUE(m.try_lock());
+  m.unlock();
+  EXPECT_TRUE(m.try_lock());
+}
+
+}  // namespace testing
+}  // namespace wpilib
diff --git a/wpilibc/wpilibC++Sim/include/DriverStation.h b/wpilibc/wpilibC++Sim/include/DriverStation.h
index 4fb92430f1..28c5713b6e 100644
--- a/wpilibc/wpilibC++Sim/include/DriverStation.h
+++ b/wpilibc/wpilibC++Sim/include/DriverStation.h
@@ -10,6 +10,8 @@
 #include "SensorBase.h"
 #include "RobotState.h"
 #include "Task.h"
+#include "HAL/cpp/priority_mutex.h"
+#include "HAL/cpp/priority_condition_variable.h"
 
 struct HALCommonControlData;
 class AnalogInput;
@@ -29,7 +31,7 @@ public:
 		kInvalid
 	};
 
-	virtual ~DriverStation();
+	virtual ~DriverStation() = default;
 	static DriverStation *GetInstance();
 	static void ReportError(std::string error);
 
@@ -117,11 +119,11 @@ private:
     void joystickCallback5(const msgs::ConstJoystickPtr &msg);
 
 	uint8_t m_digitalOut = 0;
-	MULTIWAIT_ID m_waitForDataSem = 0;
-	MUTEX_ID m_waitForDataMutex;
-	MUTEX_ID m_stateSemaphore;
-	MUTEX_ID m_joystickSemaphore;
-	double m_approxMatchTimeOffset = -1.0;
+	priority_condition_variable m_waitForDataCond;
+	priority_mutex m_waitForDataMutex;
+	mutable priority_recursive_mutex m_stateMutex;
+	priority_recursive_mutex m_joystickMutex;
+	double m_approxMatchTimeOffset = 0;
 	bool m_userInDisabled = false;
 	bool m_userInAutonomous = false;
 	bool m_userInTeleop = false;
diff --git a/wpilibc/wpilibC++Sim/include/MotorSafetyHelper.h b/wpilibc/wpilibC++Sim/include/MotorSafetyHelper.h
index bea8e02d8e..6933aac8c3 100644
--- a/wpilibc/wpilibC++Sim/include/MotorSafetyHelper.h
+++ b/wpilibc/wpilibC++Sim/include/MotorSafetyHelper.h
@@ -6,7 +6,7 @@
 #pragma once
 
 #include "ErrorBase.h"
-#include "HAL/cpp/Synchronized.hpp"
+#include "HAL/cpp/priority_mutex.h"
 
 class MotorSafety;
 
@@ -27,9 +27,9 @@ private:
 	double m_expiration;			// the expiration time for this object
 	bool m_enabled;					// true if motor safety is enabled for this motor
 	double m_stopTime; 				// the FPGA clock value when this motor has expired
-	mutable ReentrantSemaphore m_syncMutex;			// protect accesses to the state for this object
+	mutable priority_recursive_mutex m_syncMutex;			// protect accesses to the state for this object
 	MotorSafety *m_safeObject;		// the object that is using the helper
 	MotorSafetyHelper *m_nextHelper; // next object in the list of MotorSafetyHelpers
 	static MotorSafetyHelper *m_headHelper; // the head of the list of MotorSafetyHelper objects
-	static ReentrantSemaphore m_listMutex;	// protect accesses to the list of helpers
+	static priority_recursive_mutex m_listMutex;	// protect accesses to the list of helpers
 };
diff --git a/wpilibc/wpilibC++Sim/include/simulation/simTime.h b/wpilibc/wpilibC++Sim/include/simulation/simTime.h
index aada938ea0..5f53ab081b 100644
--- a/wpilibc/wpilibC++Sim/include/simulation/simTime.h
+++ b/wpilibc/wpilibC++Sim/include/simulation/simTime.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include "HAL/Semaphore.hpp"
+
 namespace wpilib { namespace internal {
     extern double simTime;
 	extern MULTIWAIT_ID time_wait;
diff --git a/wpilibc/wpilibC++Sim/src/DriverStation.cpp b/wpilibc/wpilibC++Sim/src/DriverStation.cpp
index cf3d4203d3..f1ea0bbef2 100644
--- a/wpilibc/wpilibC++Sim/src/DriverStation.cpp
+++ b/wpilibc/wpilibC++Sim/src/DriverStation.cpp
@@ -5,7 +5,6 @@
 /*----------------------------------------------------------------------------*/
 
 #include "DriverStation.h"
-#include "HAL/cpp/Synchronized.hpp"
 #include "Timer.h"
 #include "simulation/MainNode.h"
 //#include "MotorSafetyHelper.h"
@@ -33,14 +32,7 @@ uint8_t DriverStation::m_updateNumber = 0;
  * 
  * This is only called once the first time GetInstance() is called
  */
-DriverStation::DriverStation()
-{
-	// Create a new semaphore
-	m_waitForDataSem = initializeMultiWait();
-	m_waitForDataMutex = initializeMutexNormal();
-	m_stateSemaphore = initializeMutexRecursive();
-	m_joystickSemaphore = initializeMutexRecursive();
-
+DriverStation::DriverStation() {
 	state = msgs::DriverStationPtr(new msgs::DriverStation());
 	stateSub = MainNode::Subscribe("~/ds/state",
 		                   &DriverStation::stateCallback, this);
@@ -67,13 +59,6 @@ DriverStation::DriverStation()
 	AddToSingletonList();
 }
 
-DriverStation::~DriverStation()
-{
-	deleteMultiWait(m_waitForDataSem);
-	deleteMutex(m_waitForDataMutex);
-	// TODO: Release m_stateSemaphore and m_joystickSemaphore?
-}
-
 /**
  * Return a pointer to the singleton DriverStation.
  */
@@ -113,13 +98,13 @@ float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis)
 		wpi_setWPIError(BadJoystickIndex);
 		return 0.0;
 	}
-	CRITICAL_REGION(m_joystickSemaphore)
+
+	std::unique_lock<priority_recursive_mutex> lock(m_joystickMutex);
 	if (joysticks[stick] == nullptr || axis >= joysticks[stick]->axes().size())
 	{
 		return 0.0;
 	}
 	return joysticks[stick]->axes(axis);
-	END_REGION
 }
 
 /**
@@ -137,13 +122,13 @@ bool DriverStation::GetStickButton(uint32_t stick, uint32_t button)
 		wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 0 and 5");
 		return false;
 	}
-	CRITICAL_REGION(m_joystickSemaphore)
+
+	std::unique_lock<priority_recursive_mutex> lock(m_joystickMutex);
 	if (joysticks[stick] == nullptr || button >= joysticks[stick]->buttons().size())
 	{
-	        return false;
+		return false;
 	}
 	return joysticks[stick]->buttons(button-1);
-	END_REGION
 }
 
 /**
@@ -161,7 +146,8 @@ short DriverStation::GetStickButtons(uint32_t stick)
 		return false;
 	}
 	short btns = 0, btnid;
-	CRITICAL_REGION(m_joystickSemaphore)
+
+	std::unique_lock<priority_recursive_mutex> lock(m_joystickMutex);
 	msgs::JoystickPtr joy = joysticks[stick];
 	for (btnid = 0; btnid < joy->buttons().size() && btnid < 12; btnid++)
 	{
@@ -171,7 +157,6 @@ short DriverStation::GetStickButtons(uint32_t stick)
 		}
 	}
 	return btns;
-	END_REGION
 }
 
 // 5V divided by 10 bits
@@ -230,9 +215,8 @@ bool DriverStation::GetDigitalOut(uint32_t channel)
 
 bool DriverStation::IsEnabled() const
 {
-    CRITICAL_REGION(m_stateSemaphore)
+	std::unique_lock<priority_recursive_mutex> lock(m_stateMutex);
     return state != nullptr ? state->enabled() : false;
-    END_REGION
 }
 
 bool DriverStation::IsDisabled() const
@@ -242,10 +226,9 @@ bool DriverStation::IsDisabled() const
 
 bool DriverStation::IsAutonomous() const
 {
-    CRITICAL_REGION(m_stateSemaphore)
+	std::unique_lock<priority_recursive_mutex> lock(m_stateMutex);
     return state != nullptr ?
       state->state() == msgs::DriverStation_State_AUTO : false;
-	END_REGION;
 }
 
 bool DriverStation::IsOperatorControl() const
@@ -255,10 +238,9 @@ bool DriverStation::IsOperatorControl() const
 
 bool DriverStation::IsTest() const
 {
-    CRITICAL_REGION(m_stateSemaphore)
+	std::unique_lock<priority_recursive_mutex> lock(m_stateMutex);
     return state != nullptr ?
       state->state() == msgs::DriverStation_State_TEST : false;
-	END_REGION;
 }
 
 /**
@@ -301,7 +283,8 @@ uint32_t DriverStation::GetLocation() const
  */
 void DriverStation::WaitForData()
 {
-	takeMultiWait(m_waitForDataSem, m_waitForDataMutex, SEMAPHORE_WAIT_FOREVER);
+	std::unique_lock<priority_mutex> lock(m_waitForDataMutex);
+	m_waitForDataCond.wait(lock);
 }
 
 /**
@@ -341,18 +324,18 @@ uint16_t DriverStation::GetTeamNumber() const
 
 void DriverStation::stateCallback(const msgs::ConstDriverStationPtr &msg)
 {
-    CRITICAL_REGION(m_stateSemaphore)
-    *state = *msg;
-	END_REGION;
-    giveMultiWait(m_waitForDataSem);
+    {
+		std::unique_lock<priority_recursive_mutex> lock(m_stateMutex); 
+    	*state = *msg;
+	}
+    m_waitForDataCond.notify_all();
 }
 
 void DriverStation::joystickCallback(const msgs::ConstJoystickPtr &msg,
                                      int i)
 {
-    CRITICAL_REGION(m_joystickSemaphore)
+	std::unique_lock<priority_recursive_mutex> lock(m_joystickMutex);
     *(joysticks[i]) = *msg;
-	END_REGION;
 }
 
 void DriverStation::joystickCallback0(const msgs::ConstJoystickPtr &msg)
diff --git a/wpilibc/wpilibC++Sim/src/MotorSafetyHelper.cpp b/wpilibc/wpilibC++Sim/src/MotorSafetyHelper.cpp
index 8af177a15b..5a0338a762 100644
--- a/wpilibc/wpilibC++Sim/src/MotorSafetyHelper.cpp
+++ b/wpilibc/wpilibC++Sim/src/MotorSafetyHelper.cpp
@@ -13,8 +13,7 @@
 
 #include <stdio.h>
 
-MotorSafetyHelper *MotorSafetyHelper::m_headHelper = nullptr;
-ReentrantSemaphore MotorSafetyHelper::m_listMutex;
+priority_recursive_mutex MotorSafetyHelper::m_listMutex;
 
 /**
  * The constructor for a MotorSafetyHelper object.
@@ -32,7 +31,7 @@ MotorSafetyHelper::MotorSafetyHelper(MotorSafety *safeObject)
 	m_expiration = DEFAULT_SAFETY_EXPIRATION;
 	m_stopTime = Timer::GetFPGATimestamp();
 
-	Synchronized sync(m_listMutex);
+	std::unique_lock<priority_recursive_mutex> sync(m_listMutex);
 	m_nextHelper = m_headHelper;
 	m_headHelper = this;
 }
@@ -40,7 +39,7 @@ MotorSafetyHelper::MotorSafetyHelper(MotorSafety *safeObject)
 
 MotorSafetyHelper::~MotorSafetyHelper()
 {
-	Synchronized sync(m_listMutex);
+	std::unique_lock<priority_recursive_mutex> sync(m_listMutex);
 	if (m_headHelper == this)
 	{
 		m_headHelper = m_nextHelper;
@@ -62,7 +61,7 @@ MotorSafetyHelper::~MotorSafetyHelper()
  */
 void MotorSafetyHelper::Feed()
 {
-	Synchronized sync(m_syncMutex);
+	std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
 	m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
 }
 
@@ -72,7 +71,7 @@ void MotorSafetyHelper::Feed()
  */
 void MotorSafetyHelper::SetExpiration(float expirationTime)
 {
-	Synchronized sync(m_syncMutex);
+	std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
 	m_expiration = expirationTime;
 }
 
@@ -82,7 +81,7 @@ void MotorSafetyHelper::SetExpiration(float expirationTime)
  */
 float MotorSafetyHelper::GetExpiration() const
 {
-	Synchronized sync(m_syncMutex);
+	std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
 	return m_expiration;
 }
 
@@ -92,7 +91,7 @@ float MotorSafetyHelper::GetExpiration() const
  */
 bool MotorSafetyHelper::IsAlive() const
 {
-	Synchronized sync(m_syncMutex);
+	std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
 	return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
 }
 
@@ -107,7 +106,7 @@ void MotorSafetyHelper::Check()
 	DriverStation *ds = DriverStation::GetInstance();
 	if (!m_enabled || ds->IsDisabled() || ds->IsTest()) return;
 
-	Synchronized sync(m_syncMutex);
+	std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
 	if (m_stopTime < Timer::GetFPGATimestamp())
 	{
 		char buf[128];
@@ -126,7 +125,7 @@ void MotorSafetyHelper::Check()
  */
 void MotorSafetyHelper::SetSafetyEnabled(bool enabled)
 {
-	Synchronized sync(m_syncMutex);
+	std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
 	m_enabled = enabled;
 }
 
@@ -137,7 +136,7 @@ void MotorSafetyHelper::SetSafetyEnabled(bool enabled)
  */
 bool MotorSafetyHelper::IsSafetyEnabled() const
 {
-	Synchronized sync(m_syncMutex);
+	std::unique_lock<priority_recursive_mutex> sync(m_syncMutex);
 	return m_enabled;
 }
 
@@ -148,7 +147,7 @@ bool MotorSafetyHelper::IsSafetyEnabled() const
  */
 void MotorSafetyHelper::CheckMotors()
 {
-	Synchronized sync(m_listMutex);
+	std::unique_lock<priority_recursive_mutex> sync(m_listMutex);
 	for (MotorSafetyHelper *msh = m_headHelper; msh != nullptr; msh = msh->m_nextHelper)
 	{
 		msh->Check();
diff --git a/wpilibc/wpilibC++Sim/src/Notifier.cpp b/wpilibc/wpilibC++Sim/src/Notifier.cpp
index 4ad630ae15..ab9cae0240 100644
--- a/wpilibc/wpilibC++Sim/src/Notifier.cpp
+++ b/wpilibc/wpilibC++Sim/src/Notifier.cpp
@@ -10,7 +10,7 @@
 #include "WPIErrors.h"
 
 Notifier *Notifier::timerQueueHead = nullptr;
-ReentrantSemaphore Notifier::queueSemaphore;
+priority_recursive_mutex Notifier::queueMutex;
 Task* Notifier::task = nullptr;
 int Notifier::refcount = 0;
 
@@ -30,9 +30,8 @@ Notifier::Notifier(TimerEventHandler handler, void *param)
 	m_period = 0;
 	m_nextEvent = nullptr;
 	m_queued = false;
-	m_handlerSemaphore = initializeSemaphore(SEMAPHORE_FULL);
 	{
-		Synchronized sync(queueSemaphore);
+		std::unique_lock<priority_recursive_mutex> sync(queueMutex);
 		// do the first time intialization of static variables
 		if (refcount == 0)
 		{
@@ -51,7 +50,7 @@ Notifier::Notifier(TimerEventHandler handler, void *param)
 Notifier::~Notifier()
 {
 	{
-		Synchronized sync(queueSemaphore);
+		std::unique_lock<priority_recursive_mutex> sync(queueMutex);
 		DeleteFromQueue();
 
 		// Delete the static variables when the last one is going away
@@ -64,9 +63,7 @@ Notifier::~Notifier()
 
 	// Acquire the semaphore; this makes certain that the handler is 
 	// not being executed by the interrupt manager.
-	takeSemaphore(m_handlerSemaphore);
-	// Delete while holding the semaphore so there can be no race.
-	deleteSemaphore(m_handlerSemaphore);
+	std::unique_lock<priority_mutex> lock(m_handlerMutex);
 }
 
 /**
@@ -92,7 +89,7 @@ void Notifier::ProcessQueue(uint32_t mask, void *params)
 	while (true)				// keep processing past events until no more
 	{
 		{
-			Synchronized sync(queueSemaphore);
+			std::unique_lock<priority_recursive_mutex> sync(queueMutex);
 			double currentTime = GetClock();
 			current = timerQueueHead;
 			if (current == nullptr || current->m_expirationTime > currentTime)
@@ -112,16 +109,16 @@ void Notifier::ProcessQueue(uint32_t mask, void *params)
 				// not periodic; removed from queue
 				current->m_queued = false;
 			}
-			// Take handler semaphore while holding queue semaphore to make sure
+			// Take handler mutex while holding queue semaphore to make sure
 			//  the handler will execute to completion in case we are being deleted.
-			takeSemaphore(current->m_handlerSemaphore);
+			current->m_handlerMutex.lock();
 		}
 
 		current->m_handler(current->m_param);	// call the event handler
-		giveSemaphore(current->m_handlerSemaphore);
+		current->m_handlerMutex.unlock();
 	}
 	// reschedule the first item in the queue
-	Synchronized sync(queueSemaphore);
+	std::unique_lock<priority_recursive_mutex> sync(queueMutex);
 	UpdateAlarm();
 }
 
@@ -212,7 +209,7 @@ void Notifier::DeleteFromQueue()
  */
 void Notifier::StartSingle(double delay)
 {
-	Synchronized sync(queueSemaphore);
+	std::unique_lock<priority_recursive_mutex> sync(queueMutex);
 	m_periodic = false;
 	m_period = delay;
 	DeleteFromQueue();
@@ -227,7 +224,7 @@ void Notifier::StartSingle(double delay)
  */
 void Notifier::StartPeriodic(double period)
 {
-	Synchronized sync(queueSemaphore);
+	std::unique_lock<priority_recursive_mutex> sync(queueMutex);
 	m_periodic = true;
 	m_period = period;
 	DeleteFromQueue();
@@ -244,11 +241,11 @@ void Notifier::StartPeriodic(double period)
 void Notifier::Stop()
 {
 	{
-		Synchronized sync(queueSemaphore);
+		std::unique_lock<priority_recursive_mutex> sync(queueMutex);
 		DeleteFromQueue();
 	}
 	// Wait for a currently executing handler to complete before returning from Stop()
-	Synchronized sync(m_handlerSemaphore);
+	std::unique_lock<priority_mutex> sync(m_handlerMutex);
 }
 
 void Notifier::Run() {
diff --git a/wpilibc/wpilibC++Sim/src/PIDController.cpp b/wpilibc/wpilibC++Sim/src/PIDController.cpp
index e929b594a0..352c45f01e 100644
--- a/wpilibc/wpilibC++Sim/src/PIDController.cpp
+++ b/wpilibc/wpilibC++Sim/src/PIDController.cpp
@@ -9,7 +9,6 @@
 #include "PIDSource.h"
 #include "PIDOutput.h"
 #include <math.h>
-#include "HAL/cpp/Synchronized.hpp"
 
 static const char *kP = "p";
 static const char *kI = "i";
@@ -31,8 +30,7 @@ static const char *kEnabled = "enabled";
  */
 PIDController::PIDController(float Kp, float Ki, float Kd,
 								PIDSource *source, PIDOutput *output,
-								float period) :
-	m_semaphore (0)
+								float period)
 {
 	Initialize(Kp, Ki, Kd, 0.0f, source, output, period);
 }
@@ -49,8 +47,7 @@ PIDController::PIDController(float Kp, float Ki, float Kd,
  */
 PIDController::PIDController(float Kp, float Ki, float Kd, float Kf,
 								PIDSource *source, PIDOutput *output,
-								float period) :
-	m_semaphore (0)
+								float period)
 {
 	Initialize(Kp, Ki, Kd, Kf, source, output, period);
 }
@@ -87,8 +84,6 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
 	m_pidOutput = output;
 	m_period = period;
 
-
-	m_semaphore = initializeMutexRecursive();
 	m_controlLoop = new Notifier(PIDController::CallCalculate, this);
 	m_controlLoop->StartPeriodic(m_period);
 
@@ -103,8 +98,6 @@ void PIDController::Initialize(float Kp, float Ki, float Kd, float Kf,
  */
 PIDController::~PIDController()
 {
-	takeMutex(m_semaphore);
-	deleteMutex(m_semaphore);
 	delete m_controlLoop;
 }
 
@@ -131,14 +124,13 @@ void PIDController::Calculate()
 	bool enabled;
 	PIDSource *pidInput;
 
-	CRITICAL_REGION(m_semaphore)
 	{
+		std::unique_lock<priority_mutex> lock(m_mutex);
 		if (m_pidInput == 0) return;
 		if (m_pidOutput == 0) return;
 		enabled = m_enabled;
 		pidInput = m_pidInput;
 	}
-	END_REGION;
 
 	if (enabled)
 	{
@@ -147,7 +139,7 @@ void PIDController::Calculate()
 		PIDOutput *pidOutput;
 
 		{
-			Synchronized sync(m_semaphore);
+			std::unique_lock<priority_mutex> sync(m_mutex);
 			m_error = m_setpoint - input;
 			if (m_continuous)
 			{
@@ -203,13 +195,12 @@ void PIDController::Calculate()
  */
 void PIDController::SetPID(double p, double i, double d)
 {
-	CRITICAL_REGION(m_semaphore)
 	{
+		std::unique_lock<priority_mutex> lock(m_mutex);
 		m_P = p;
 		m_I = i;
 		m_D = d;
 	}
-	END_REGION;
 
 	if (m_table != nullptr) {
 		m_table->PutNumber("p", m_P);
@@ -228,14 +219,13 @@ void PIDController::SetPID(double p, double i, double d)
  */
 void PIDController::SetPID(double p, double i, double d, double f)
 {
-	CRITICAL_REGION(m_semaphore)
 	{
+		std::unique_lock<priority_mutex> lock(m_mutex);
 		m_P = p;
 		m_I = i;
 		m_D = d;
 		m_F = f;
 	}
-	END_REGION;
 
 	if (m_table != nullptr) {
 		m_table->PutNumber("p", m_P);
@@ -251,11 +241,8 @@ void PIDController::SetPID(double p, double i, double d, double f)
  */
 double PIDController::GetP() const
 {
-	CRITICAL_REGION(m_semaphore)
-	{
-		return m_P;
-	}
-	END_REGION;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	return m_P;
 }
 
 /**
@@ -264,11 +251,8 @@ double PIDController::GetP() const
  */
 double PIDController::GetI() const
 {
-	CRITICAL_REGION(m_semaphore)
-	{
-		return m_I;
-	}
-	END_REGION;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	return m_I;
 }
 
 /**
@@ -277,11 +261,8 @@ double PIDController::GetI() const
  */
 double PIDController::GetD() const
 {
-	CRITICAL_REGION(m_semaphore)
-	{
-		return m_D;
-	}
-	END_REGION;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	return m_D;
 }
 
 /**
@@ -290,11 +271,8 @@ double PIDController::GetD() const
  */
 double PIDController::GetF() const
 {
-	CRITICAL_REGION(m_semaphore)
-	{
-		return m_F;
-	}
-	END_REGION;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	return m_F;
 }
 
 /**
@@ -304,13 +282,8 @@ double PIDController::GetF() const
  */
 float PIDController::Get() const
 {
-	float result;
-	CRITICAL_REGION(m_semaphore)
-	{
-		result = m_result;
-	}
-	END_REGION;
-	return result;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	return m_result;
 }
 
 /**
@@ -322,12 +295,8 @@ float PIDController::Get() const
  */
 void PIDController::SetContinuous(bool continuous)
 {
-	CRITICAL_REGION(m_semaphore)
-	{
-		m_continuous = continuous;
-	}
-	END_REGION;
-
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	m_continuous = continuous;
 }
 
 /**
@@ -338,12 +307,11 @@ void PIDController::SetContinuous(bool continuous)
  */
 void PIDController::SetInputRange(float minimumInput, float maximumInput)
 {
-	CRITICAL_REGION(m_semaphore)
 	{
+		std::unique_lock<priority_mutex> lock(m_mutex);
 		m_minimumInput = minimumInput;
 		m_maximumInput = maximumInput;
 	}
-	END_REGION;
 
 	SetSetpoint(m_setpoint);
 }
@@ -356,12 +324,9 @@ void PIDController::SetInputRange(float minimumInput, float maximumInput)
  */
 void PIDController::SetOutputRange(float minimumOutput, float maximumOutput)
 {
-	CRITICAL_REGION(m_semaphore)
-	{
-		m_minimumOutput = minimumOutput;
-		m_maximumOutput = maximumOutput;
-	}
-	END_REGION;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	m_minimumOutput = minimumOutput;
+	m_maximumOutput = maximumOutput;
 }
 
 /**
@@ -370,8 +335,8 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput)
  */
 void PIDController::SetSetpoint(float setpoint)
 {
-	CRITICAL_REGION(m_semaphore)
 	{
+		std::unique_lock<priority_mutex> lock(m_mutex);
 		if (m_maximumInput > m_minimumInput)
 		{
 			if (setpoint > m_maximumInput)
@@ -386,8 +351,7 @@ void PIDController::SetSetpoint(float setpoint)
 			m_setpoint = setpoint;
 		}
 	}
-	END_REGION;	
-	
+
 	if (m_table != nullptr) {
 		m_table->PutNumber("setpoint", m_setpoint);
 	}
@@ -399,13 +363,8 @@ void PIDController::SetSetpoint(float setpoint)
  */
 double PIDController::GetSetpoint() const
 {
-	float setpoint;
-	CRITICAL_REGION(m_semaphore)
-	{
-		setpoint = m_setpoint;
-	}
-	END_REGION;
-	return setpoint;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	return m_setpoint;
 }
 
 /**
@@ -414,13 +373,12 @@ double PIDController::GetSetpoint() const
  */
 float PIDController::GetError() const
 {
-	float error;
-	CRITICAL_REGION(m_semaphore)
+	double pidInput;
 	{
-		error = m_setpoint - m_pidInput->PIDGet();
+		std::unique_lock<priority_mutex> lock(m_mutex);
+		pidInput = m_pidInput->PIDGet();
 	}
-	END_REGION;
-	return error;
+	return GetSetpoint() - pidInput;
 }
 
 /*
@@ -430,12 +388,9 @@ float PIDController::GetError() const
  */
 void PIDController::SetTolerance(float percent)
 {
-	CRITICAL_REGION(m_semaphore)
-	{
-		m_toleranceType = kPercentTolerance;
-		m_tolerance = percent;
-	}
-	END_REGION;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	m_toleranceType = kPercentTolerance;
+	m_tolerance = percent;
 }
 
 /*
@@ -445,12 +400,9 @@ void PIDController::SetTolerance(float percent)
  */
 void PIDController::SetPercentTolerance(float percent)
 {
-	CRITICAL_REGION(m_semaphore)
-	{
-		m_toleranceType = kPercentTolerance;
-		m_tolerance = percent;
-	}
-	END_REGION;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	m_toleranceType = kPercentTolerance;
+	m_tolerance = percent;
 }
 
 /*
@@ -460,12 +412,9 @@ void PIDController::SetPercentTolerance(float percent)
  */
 void PIDController::SetAbsoluteTolerance(float absTolerance)
 {
-	CRITICAL_REGION(m_semaphore)
-	{
-		m_toleranceType = kAbsoluteTolerance;
-		m_tolerance = absTolerance;
-	}
-	END_REGION;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	m_toleranceType = kAbsoluteTolerance;
+	m_tolerance = absTolerance;
 }
 
 /*
@@ -477,22 +426,20 @@ void PIDController::SetAbsoluteTolerance(float absTolerance)
  */
 bool PIDController::OnTarget() const
 {
-	bool temp;
-	CRITICAL_REGION(m_semaphore)
-	{
-		switch (m_toleranceType) {
-		case kPercentTolerance:
-			temp = fabs(GetError()) < (m_tolerance / 100 * (m_maximumInput - m_minimumInput));
-			break;
-		case kAbsoluteTolerance:
-			temp = fabs(GetError()) < m_tolerance;
-			break;
-		case kNoTolerance: // TODO: this case needs an error
-			temp = false;
-		}
+	double error = GetError();
+
+	std::unique_lock<priority_mutex> sync(m_mutex);
+	switch (m_toleranceType) {
+	case kPercentTolerance:
+		return fabs(error) < m_tolerance / 100 * (m_maximumInput - m_minimumInput);
+		break;
+	case kAbsoluteTolerance:
+		return fabs(error) < m_tolerance;
+		break;
+	case kNoTolerance: // TODO: this case needs an error
+		return false;
 	}
-	END_REGION;
-	return temp;
+	return false;
 }
 
 /**
@@ -500,12 +447,11 @@ bool PIDController::OnTarget() const
  */
 void PIDController::Enable()
 {
-	CRITICAL_REGION(m_semaphore)
 	{
+		std::unique_lock<priority_mutex> lock(m_mutex);
 		m_enabled = true;
 	}
-	END_REGION;	
-	
+
 	if (m_table != nullptr) {
 		m_table->PutBoolean("enabled", true);
 	}
@@ -516,13 +462,12 @@ void PIDController::Enable()
  */
 void PIDController::Disable()
 {
-	CRITICAL_REGION(m_semaphore)
 	{
+		std::unique_lock<priority_mutex> lock(m_mutex);
 		m_pidOutput->PIDWrite(0);
 		m_enabled = false;
 	}
-	END_REGION;
-	
+
 	if (m_table != nullptr) {
 		m_table->PutBoolean("enabled", false);
 	}
@@ -533,13 +478,8 @@ void PIDController::Disable()
  */
 bool PIDController::IsEnabled() const
 {
-	bool enabled;
-	CRITICAL_REGION(m_semaphore)
-	{
-		enabled = m_enabled;
-	}
-	END_REGION;
-	return enabled;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	return m_enabled;
 }
 
 /**
@@ -549,13 +489,10 @@ void PIDController::Reset()
 {
 	Disable();
 
-	CRITICAL_REGION(m_semaphore)
-	{
-		m_prevError = 0;
-		m_totalError = 0;
-		m_result = 0;
-	}
-	END_REGION;
+	std::unique_lock<priority_mutex> lock(m_mutex);
+	m_prevError = 0;
+	m_totalError = 0;
+	m_result = 0;
 }
 
 std::string PIDController::GetSmartDashboardType() const {
diff --git a/wpilibc/wpilibC++Sim/src/Task.cpp b/wpilibc/wpilibC++Sim/src/Task.cpp
index 80a212e08d..2af6bb293d 100644
--- a/wpilibc/wpilibC++Sim/src/Task.cpp
+++ b/wpilibc/wpilibC++Sim/src/Task.cpp
@@ -11,6 +11,13 @@
 #include <string.h>
 #include <stdio.h>
 
+#ifndef OK
+#define OK 0
+#endif /* OK */
+#ifndef ERROR
+#define ERROR (-1)
+#endif /* ERROR */
+
 const uint32_t Task::kDefaultPriority;
 
 /**
diff --git a/wpilibc/wpilibC++Sim/src/Timer.cpp b/wpilibc/wpilibC++Sim/src/Timer.cpp
index 4d6bba49f8..2bdf4ba349 100644
--- a/wpilibc/wpilibC++Sim/src/Timer.cpp
+++ b/wpilibc/wpilibC++Sim/src/Timer.cpp
@@ -8,7 +8,6 @@
 
 #include <time.h>
 
-#include "HAL/cpp/Synchronized.hpp"
 #include "simulation/simTime.h"
 #include "Utility.h"
 
@@ -62,19 +61,12 @@ Timer::Timer()
 	: m_startTime (0.0)
 	, m_accumulatedTime (0.0)
 	, m_running (false)
-	, m_semaphore (0)
 {
 	//Creates a semaphore to control access to critical regions.
 	//Initially 'open'
-	m_semaphore = initializeMutexNormal();
 	Reset();
 }
 
-Timer::~Timer()
-{
-	deleteMutex(m_semaphore);
-}
-
 /**
  * Get the current time from the timer. If the clock is running it is derived from
  * the current system clock the start time stored in the timer class. If the clock
@@ -87,7 +79,7 @@ double Timer::Get() const
 	double result;
 	double currentTime = GetFPGATimestamp();
 
-	Synchronized sync(m_semaphore);
+	std::unique_lock<priority_mutex> sync(m_mutex);
 	if(m_running)
 	{
 		// This math won't work if the timer rolled over (71 minutes after boot).
@@ -109,7 +101,7 @@ double Timer::Get() const
  */
 void Timer::Reset()
 {
-	Synchronized sync(m_semaphore);
+	std::unique_lock<priority_mutex> sync(m_mutex);
 	m_accumulatedTime = 0;
 	m_startTime = GetFPGATimestamp();
 }
@@ -121,7 +113,7 @@ void Timer::Reset()
  */
 void Timer::Start()
 {
-	Synchronized sync(m_semaphore);
+	std::unique_lock<priority_mutex> sync(m_mutex);
 	if (!m_running)
 	{
 		m_startTime = GetFPGATimestamp();
@@ -139,7 +131,7 @@ void Timer::Stop()
 {
 	double temp = Get();
 
-	Synchronized sync(m_semaphore);
+	std::unique_lock<priority_mutex> sync(m_mutex);
 	if (m_running)
 	{
 		m_accumulatedTime = temp;
@@ -159,7 +151,7 @@ bool Timer::HasPeriodPassed(double period)
 {
 	if (Get() > period)
 	{
-		Synchronized sync(m_semaphore);
+		std::unique_lock<priority_mutex> sync(m_mutex);
 		// Advance the start time by the period.
 		// Don't set it to the current time... we want to avoid drift.
 		m_startTime += period;
@@ -203,8 +195,6 @@ extern "C"
 #include "simulation/MainNode.h"
 namespace wpilib { namespace internal {
     double simTime = 0;
-    MULTIWAIT_ID time_wait = initializeMultiWait();
-    MUTEX_ID time_wait_mutex = initializeMutexNormal();
 
     void time_callback(const msgs::ConstFloat64Ptr &msg) {
         simTime = msg->data();
diff --git a/wpilibj/wpilibJavaJNI/lib/HALUtil.cpp b/wpilibj/wpilibJavaJNI/lib/HALUtil.cpp
index 5a613be155..b9a46f7027 100644
--- a/wpilibj/wpilibJavaJNI/lib/HALUtil.cpp
+++ b/wpilibj/wpilibJavaJNI/lib/HALUtil.cpp
@@ -33,10 +33,10 @@ JNIEXPORT jobject JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_initializeMutex
 (JNIEnv * env, jclass)
 {
 	HALUTIL_LOG(logDEBUG) << "Calling HALUtil initializeMutex";
-	MUTEX_ID* mutexPtr = (MUTEX_ID*)new unsigned char[4];
+	MUTEX_ID* mutexPtr = (MUTEX_ID*)new unsigned char[sizeof(MUTEX_ID)];
 	*mutexPtr = initializeMutexNormal();
 	HALUTIL_LOG(logDEBUG) << "Mutex Ptr = " << *mutexPtr;
-	return env->NewDirectByteBuffer( mutexPtr, 4);
+	return env->NewDirectByteBuffer(mutexPtr, sizeof(MUTEX_ID));
 }
 
 /*
@@ -51,6 +51,7 @@ JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_HALUtil_deleteMutex
 	MUTEX_ID* javaId = (MUTEX_ID*)env->GetDirectBufferAddress(id);
 	HALUTIL_LOG(logDEBUG) << "Mutex Ptr = " << *javaId;
 	deleteMutex( *javaId );
+	delete[] javaId;
 }
 
 /*