diff --git a/include/defines.h b/include/defines.h
index 82bf3e1..e41d3da 100644
--- a/include/defines.h
+++ b/include/defines.h
@@ -35,32 +35,6 @@
 
 #define servoPin GPIO_NUM_20
 #define servoLEDCChannel LEDC_CHANNEL_0
-
-#define getMovingCW(port) ((movingCW & (1 << port)) >> port)
-#define setMovingCW(port) (movingCW |= (1 << port))
-#define clearMovingCW(port) (movingCW &= ~(1 << port))
-#define getMovingCCW(port) ((movingCCW & (1 << port)) >> port)
-#define setMovingCCW(port) (movingCCW |= (1 << port))
-#define clearMovingCCW(port) (movingCCW &= ~(1 << port))
-#define getCalibCW(port) ((calibCW & (1 << port)) >> port)
-#define setCalibCW(port) (calibCW |= (1 << port))
-#define clearCalibCW(port) (calibCW &= ~(1 << port))
-#define getCalibCCW(port) ((calibCCW & (1 << port)) >> port)
-#define setCalibCCW(port) (calibCCW |= (1 << port))
-#define clearCalibCCW(port) (calibCCW &= ~(1 << port))
-#define getCalibDone(port) ((calibDone & (1 << port)) >> port)
-#define setCalibDone(port) (calibDone |= (1 << port))
-#define clearCalibDone(port) (calibDone &= ~(1 << port))
-#define getBlocked(port) ((blocked & (1 << port)) >> port)
-#define setBlocked(port) (blocked |= (1 << port))
-#define clearBlocked(port) (blocked &= ~(1 << port))
-#define getPos10(port) ((movingCCW & (1 << (port + 4))) >> (port + 4))
-#define setPos10(port) (movingCCW |= (1 << (port + 4)))
-#define clearPos10(port) (movingCCW &= ~(1 << (port + 4)))
-#define getPos0(port) ((movingCW & (1 << (port + 4))) >> (port + 4))
-#define setPos0(port) (movingCW |= (1 << (port + 4)))
-#define clearPos0(port) (movingCW &= ~(1 << (port + 4)))
-
-#define prefNameCalibs "periph_info_"
+#define servoSwitch GPIO_NUM_17
 
 #endif
\ No newline at end of file
diff --git a/include/servo.cpp b/include/servo.cpp
index 364a761..76dcbc7 100644
--- a/include/servo.cpp
+++ b/include/servo.cpp
@@ -43,6 +43,10 @@ void servoInit(Encoder& bottom, Encoder& top) {
   topEnc = &top;
   bottomEnc = &bottom;
 
+  // Configure servo power switch pin as output
+  gpio_set_direction(servoSwitch, GPIO_MODE_OUTPUT);
+  gpio_set_level(servoSwitch, 0); // Start with servo power off
+
   topEnc->count = servoReadPos();
   if (calib.getCalibrated()) initMainLoop();
 }
@@ -58,13 +62,13 @@ void servoOn(uint8_t dir, uint8_t manOrServer) {
 void servoOff() {
   ledc_set_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel, offSpeed);
   ledc_update_duty(LEDC_LOW_SPEED_MODE, servoLEDCChannel);
-  servoMainSwitch(0);
   runningManual = false;
   runningServer = false;
+  servoMainSwitch(0);
 }
 
 void servoMainSwitch(uint8_t onOff) {
-  // To Be Implemented
+  gpio_set_level(servoSwitch, onOff ? 1 : 0);
 }
 
 bool servoInitCalib() {