diff --git a/Firmware/EggbotWireless/include/Pen.h b/Firmware/EggbotWireless/include/Pen.h
index 2735079..ae8ec90 100644
--- a/Firmware/EggbotWireless/include/Pen.h
+++ b/Firmware/EggbotWireless/include/Pen.h
@@ -3,13 +3,13 @@
 #ifndef PEN_H
 #define PEN_H
 
-#define DELAY 300
+#define DELAY 15
 
 class Pen {
    private:
     int posEngaged;
     int posDisengaged;
-    bool engaged;
+    bool engaged = true;
     Servo servo;
 
    public:
diff --git a/Firmware/EggbotWireless/include/Stepper.h b/Firmware/EggbotWireless/include/Stepper.h
index 3e00d00..6cfb31d 100644
--- a/Firmware/EggbotWireless/include/Stepper.h
+++ b/Firmware/EggbotWireless/include/Stepper.h
@@ -23,6 +23,7 @@ class Stepper {
     Stepper(int pin1, int pin2, int pin3, int pin4, int stepsPerRevolution, int backlashSteps, int limit);
     void rotate(float degrees);
     void moveTo(float degrees);
+    void setPos(float degrees);
     float getDist(float degrees);
     void doStep();
     bool finished();
diff --git a/Firmware/EggbotWireless/src/Pen.cpp b/Firmware/EggbotWireless/src/Pen.cpp
index 337ce37..ee4b9d7 100644
--- a/Firmware/EggbotWireless/src/Pen.cpp
+++ b/Firmware/EggbotWireless/src/Pen.cpp
@@ -7,19 +7,25 @@ Pen::Pen(int pin, int posEngaged, int posDisengaged)
 }
 
 void Pen::engage() {
-    servo.write(posEngaged);
+    if (!engaged) {
+        for (int i = posDisengaged; i > posEngaged; i--) {
+            servo.write(i);
+            delay(DELAY);
+        }
+    }
     engaged = true;
-    delay(DELAY);
 }
 
 void Pen::disengage() {
-    servo.write(posDisengaged);
+    if (engaged) {
+        for (int i = posEngaged; i < posDisengaged; i++) {
+            servo.write(i);
+            delay(DELAY);
+        }
+    }
     engaged = false;
-    delay(DELAY);
 }
 
-bool Pen::getEngaged() {
-    return engaged;
-}
+bool Pen::getEngaged() { return engaged; }
 
 Pen::~Pen() {}
diff --git a/Firmware/EggbotWireless/src/Stepper.cpp b/Firmware/EggbotWireless/src/Stepper.cpp
index 2470603..97dce6e 100644
--- a/Firmware/EggbotWireless/src/Stepper.cpp
+++ b/Firmware/EggbotWireless/src/Stepper.cpp
@@ -196,5 +196,9 @@ void Stepper::moveTo(float degrees) {
     rotate(mod - pos);
 }
 
+void Stepper::setPos(float degrees) {
+    pos = degrees;
+}
+
 bool Stepper::finished() { return remainingSteps == 0; }
 float Stepper::getPos() { return pos; }
\ No newline at end of file
diff --git a/Firmware/EggbotWireless/src/main.cpp b/Firmware/EggbotWireless/src/main.cpp
index 63d70a0..a8dc880 100644
--- a/Firmware/EggbotWireless/src/main.cpp
+++ b/Firmware/EggbotWireless/src/main.cpp
@@ -6,12 +6,12 @@
 #include "StepperTimer.h"
 
 #define STEPS_PER_REVOLUTION 4076
-#define BACKLASH_STEPS 20
-int RPM = 10;
+#define BACKLASH_STEPS 40
+int RPM = 4;
 
 Stepper eggStepper(D1, D2, D3, D4, STEPS_PER_REVOLUTION, BACKLASH_STEPS, 0);
-Stepper servoStepper(D5, D6, D7, D8, STEPS_PER_REVOLUTION, BACKLASH_STEPS, 80);
-Pen pen(D0, 80, 170);
+Stepper servoStepper(D5, D6, D7, D8, -STEPS_PER_REVOLUTION, BACKLASH_STEPS, 70);
+Pen pen(D0, 100, 170);
 
 StepperTimer eggStepperTimer(RPM, STEPS_PER_REVOLUTION);
 StepperTimer servoStepperTimer(RPM, STEPS_PER_REVOLUTION);
@@ -41,10 +41,10 @@ void execCommand(int *command) {
             eggStepper.moveTo(command[Y]);
         }
 
-        if (command[Z] == 1) {
+        if (command[Z] < 0) {
             pen.engage();
         }
-        if (command[Z] == 0) {
+        if (command[Z] >= 0) {
             pen.disengage();
         }
 
@@ -66,13 +66,13 @@ void setup() {
     eggStepperTimer.setStepper(&eggStepper);
     servoStepperTimer.setStepper(&servoStepper);
     pen.disengage();
+    servoStepper.setPos(70);
     WiFi.mode(WIFI_OFF);
 }
 
 void loop() {
     while (Serial.available() > 0) {
         char inChar = Serial.read();
-        Serial.write(inChar);
         inString += inChar;
 
         if (inChar == '\n') {