diff --git a/Kabob_Code/platformio.ini b/Kabob_Code/platformio.ini
index 95d9947..e4181f5 100644
--- a/Kabob_Code/platformio.ini
+++ b/Kabob_Code/platformio.ini
@@ -18,6 +18,7 @@ board = teensylc
 framework = arduino
 lib_deps =
   https://github.com/PaulStoffregen/Encoder.git
+  arduino-libraries/Stepper@^1.1.3
 lib_ignore = 
   FreeRTOS-Kernel
 
diff --git a/Kabob_Code/src/Actuators/Chassis.cpp b/Kabob_Code/src/Actuators/Chassis.cpp
index b2eb3a9..9921d30 100644
--- a/Kabob_Code/src/Actuators/Chassis.cpp
+++ b/Kabob_Code/src/Actuators/Chassis.cpp
@@ -10,15 +10,15 @@ void Chassis::stop(void)
 void Chassis::move_forward(double distance, uint8_t speed)
 {
   double movement_degrees = distance / wheel_circumference * 360;
-  right.cw_at_speed(speed);
-  left.ccw_at_speed(speed);
+  right.cw(movement_degrees, speed);
+  left.ccw(-movement_degrees, speed);
 }
 
 void Chassis::move_backward(double distance, uint8_t speed)
 {
   double movement_degrees = distance / wheel_circumference * 360;
-  right.ccw_at_speed(speed);
-  left.cw_at_speed(speed);
+  right.cw(-movement_degrees, speed);
+  left.ccw(movement_degrees, speed);
 }
 
 void Chassis::turn_right(uint8_t speed)
@@ -47,16 +47,16 @@ void Chassis::veer_backward(uint8_t speed_right, uint8_t speed_left)
 
 void Chassis::turn_cw(double angle, uint8_t speed)
 {
-  double movement_degrees = chassis_circumference / (angle / 360) / wheel_circumference * 360;
-  right.ccw_at_speed(speed);
-  left.ccw_at_speed(speed);
+  double movement_degrees = angle * chassis_circumference / wheel_circumference;
+  right.ccw(movement_degrees, speed);
+  left.ccw(movement_degrees, speed);
 }
 
 void Chassis::turn_ccw(double angle, uint8_t speed)
 {
-  double movement_degrees = chassis_circumference / (angle / 360) / wheel_circumference * 360;
-  right.cw_at_speed(speed);
-  left.cw_at_speed(speed);
+  double movement_degrees = angle * chassis_circumference / wheel_circumference;
+  right.cw(movement_degrees, speed);
+  left.cw(movement_degrees, speed);
 }
 
 void Chassis::move_forward_at_speed(uint8_t speed)
diff --git a/Kabob_Code/src/Actuators/Chassis.h b/Kabob_Code/src/Actuators/Chassis.h
index c3da426..34a4f6a 100644
--- a/Kabob_Code/src/Actuators/Chassis.h
+++ b/Kabob_Code/src/Actuators/Chassis.h
@@ -2,6 +2,8 @@
 
 #include "Arduino.h"
 #include "Motor.h"
+#include "MotorWithEncoder.h"
+#include "StepperMotor.h"
 
 class Chassis
 {
@@ -16,8 +18,11 @@ class Chassis
           unsigned le_pin2,
           double hub_to_hub_distance,
           double wheel_circumference,
-          uint8_t min_speed) : right(rf_pin, rb_pin, min_speed),
-                               left(lf_pin, lb_pin, min_speed),
+          uint8_t min_speed,
+          unsigned dir_pin,
+          unsigned step_pin) : right(rf_pin, rb_pin, re_pin1, re_pin2, min_speed),
+                               left(lf_pin, lb_pin, le_pin1, le_pin2, min_speed),
+                               stepper(dir_pin, step_pin),
                                chassis_circumference(hub_to_hub_distance * PI),
                                wheel_circumference(wheel_circumference),
                                ninety_degrees(chassis_circumference / 4 / wheel_circumference * 360){};
@@ -29,14 +34,14 @@ class Chassis
    * distance (centimeters)
    * - if stop() is called, the Chassis will reset its target.
    */
-  void move_forward(double distance, uint8_t speed = 50);
-  void move_backward(double distance, uint8_t speed = 50);
+  void move_forward(double distance, uint8_t speed = 150);
+  void move_backward(double distance, uint8_t speed = 150);
 
   /* turn_right and turn_left
    * rotate 90°
    */
-  void turn_right(uint8_t speed = 50);
-  void turn_left(uint8_t speed = 50);
+  void turn_right(uint8_t speed = 150);
+  void turn_left(uint8_t speed = 150);
 
   void veer_forward(uint8_t speed_right, uint8_t speed_left);
   void veer_backward(uint8_t speed_right, uint8_t speed_left);
@@ -45,19 +50,22 @@ class Chassis
    * distance (centimeters)
    * - if stop() is called, the Chassis will reset its target.
    */
-  void turn_cw(double angle, uint8_t speed = 50);
-  void turn_ccw(double angle, uint8_t speed = 50);
+  void turn_cw(double angle, uint8_t speed = 150);
+  void turn_ccw(double angle, uint8_t speed = 150);
 
   void move_forward_at_speed(uint8_t speed);
   void move_backward_at_speed(uint8_t speed);
   void turn_right_at_speed(uint8_t speed);
   void turn_left_at_speed(uint8_t speed);
 
-  Motor right;
-  Motor left;
+  MotorWithEncoder right;
+  MotorWithEncoder left;
+
+  StepperMotor stepper;
 
 private:
   double chassis_circumference,
       wheel_circumference,
       ninety_degrees;
+  uint8_t executing;
 };
\ No newline at end of file
diff --git a/Kabob_Code/src/Actuators/MotorWithEncoder.cpp b/Kabob_Code/src/Actuators/MotorWithEncoder.cpp
index cb05f73..07c0e1f 100644
--- a/Kabob_Code/src/Actuators/MotorWithEncoder.cpp
+++ b/Kabob_Code/src/Actuators/MotorWithEncoder.cpp
@@ -1,7 +1,7 @@
 #include "Arduino.h"
 #include "MotorWithEncoder.h"
 
-void MotorWithEncoder::activity(){
+void MotorWithEncoder::activity(){ //TODO schedule with interrupt at some frequency
   pos = abs(encoder.read());
   
   // overflow protection
@@ -24,9 +24,13 @@ void MotorWithEncoder::stop(void) {
   overflow = 0;
   pos = 0;
   target = 0;
+  occupied = false;
 }
 
 void MotorWithEncoder::move(double angle, uint8_t speed){
+  if (occupied) return;
+  occupied = true;
+
   if (angle > 0){
     cw_at_speed(speed);
   } else {
diff --git a/Kabob_Code/src/Actuators/MotorWithEncoder.h b/Kabob_Code/src/Actuators/MotorWithEncoder.h
index dd8f537..1203d28 100644
--- a/Kabob_Code/src/Actuators/MotorWithEncoder.h
+++ b/Kabob_Code/src/Actuators/MotorWithEncoder.h
@@ -6,7 +6,7 @@
 #define ENCODER_OPTIMIZE_INTERRUPTS
 #include <Encoder.h>
 
-class MotorWithEncoder : Motor
+class MotorWithEncoder : public Motor
 {
 public:
   MotorWithEncoder(unsigned f_pin,
@@ -42,4 +42,5 @@ class MotorWithEncoder : Motor
   double target;
   double overflow;
   uint8_t min_speed;
+  uint8_t occupied;
 };
\ No newline at end of file
diff --git a/Kabob_Code/src/Actuators/StepperMotor.cpp b/Kabob_Code/src/Actuators/StepperMotor.cpp
new file mode 100644
index 0000000..e450f30
--- /dev/null
+++ b/Kabob_Code/src/Actuators/StepperMotor.cpp
@@ -0,0 +1,17 @@
+#include "Arduino.h"
+#include "StepperMotor.h"
+
+void StepperMotor::cw(int steps) {
+  stepper.setSpeed(60);
+  stepper.step(steps);
+}
+
+void StepperMotor::ccw(int steps) {
+  stepper.setSpeed(-60);
+  stepper.step(steps);
+}
+
+void StepperMotor::stop() {
+    return;
+}
+
diff --git a/Kabob_Code/src/Actuators/StepperMotor.h b/Kabob_Code/src/Actuators/StepperMotor.h
new file mode 100644
index 0000000..4d3a99d
--- /dev/null
+++ b/Kabob_Code/src/Actuators/StepperMotor.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include "Arduino.h"
+#include "Stepper.h"
+
+#define STEPS_PER_REVOLUTION 200
+
+class StepperMotor
+{
+public:
+  StepperMotor(unsigned dir_pin, unsigned step_pin)
+  {
+    pinMode(dir_pin, OUTPUT);
+    pinMode(step_pin, OUTPUT);
+    stepper = Stepper(STEPS_PER_REVOLUTION, dir_pin, step_pin);
+
+  };
+
+  void cw(int steps);
+  void ccw(int steps);
+  void stop(void);
+
+  Stepper stepper;
+
+};
diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp
index 884734b..d46dea6 100644
--- a/Kabob_Code/src/StateMachine/StateMachine.cpp
+++ b/Kabob_Code/src/StateMachine/StateMachine.cpp
@@ -1,9 +1,11 @@
 #include "StateMachine.h"
 
-#define VELOCITY 255
+#define VELOCITY 100
 
-#define Right_TURN_TIME       4000
-#define SECOUND_TURN_TIME     2500
+#define Right_TURN_TIME       3000
+#define SECOUND_TURN_TIME     1000
+
+uint8_t DEBUG = true; 
 
 States_t state;
 States_r line_state;
@@ -20,6 +22,14 @@ uint8_t servoPos = 0;
 uint8_t flagLeftLine = 0;
 uint8_t flagRightLine = 0;
 
+
+// right now code only works for state red
+uint8_t stateRed = true;
+
+//if this is true, do not allow motor to be set on different course
+// unless extreme reason (i.e. hole)
+uint8_t navigationSet = 0;
+
 unsigned long flag_time;
 
 void setup() {
@@ -27,16 +37,14 @@ void setup() {
   Serial.begin(9600);
   shephard.claw.open();
   while(!Serial);
-  //delay(10000);
-  delay(3000);
+  delay(5000);
+
   //times
   curr_time = millis();
   serial_time = millis();
   state_time = millis();
   flag_time = millis();
 
-  
-
   changeStateTo(STATE_LOAD);
   
   Serial.println("Setup Complete!");
@@ -47,8 +55,7 @@ void loop() {
   checkGlobalEvents();
   checkFlags();
   checkForZoneChange();
-
-  // TO LINE FOLLOW: Must be in state = state_nav_target
+  shephard.activity();
 
   switch(state) {
     case STATE_IDLE:
@@ -71,14 +78,14 @@ void loop() {
   }
 
   //debug for line sensors
-  if (curr_time - serial_time > MILLISECONDS(1/PRINT_FREQUENCY) ){
+  if (curr_time - serial_time > MILLISECONDS(1/PRINT_FREQUENCY) && DEBUG){
+    Serial.println("---------------");
+    Serial.println("left " + String(shephard.sensors.line.left.read()));
+    Serial.println("right " + String(shephard.sensors.line.right.read()));
+    Serial.println("center left " + String(shephard.sensors.line.center_left.read()));
+    Serial.println("center middle " + String(shephard.sensors.line.center_middle.read()));
+    Serial.println("center right " + String(shephard.sensors.line.center_right.read()));
     Serial.println("---------------");
-    //Serial.println("left " + String(shephard.sensors.line.left.read()));
-    //Serial.println("right " + String(shephard.sensors.line.right.read()));
-    //Serial.println("center left " + String(shephard.sensors.line.center_left.read()));
-    //Serial.println("center middle " + String(shephard.sensors.line.center_middle.read()));
-    //Serial.println("center right " + String(shephard.sensors.line.center_right.read()));
-    //Serial.println("---------------");
     serial_time = curr_time;
   }
 }
@@ -89,9 +96,9 @@ void handleLoadState(void) {
     shephard.claw.open(); 
   } else if (timeInState > 1000 && timeInState < 2000) {
     shephard.claw.close(); 
-  } else if (timeInState > 2000  && timeInState < 5000) {
-    shephard.chassis.move_forward_at_speed(VELOCITY);
-  } else if (timeInState > 5000) { 
+  } else if (timeInState > 2000  && timeInState < 2100) {
+    shephard.chassis.move_forward(20, 150);
+  } else if (timeInState > 3500) { 
     changeStateTo(STATE_NAV_TARGET);
     changeZoneTo(ZONE_1);
   }
@@ -102,9 +109,9 @@ void handleUnloadState(void) {
   if (timeInState < 1000) {
     shephard.claw.open(); 
   } else if (timeInState > 1000 && timeInState < 4000) {
-    shephard.chassis.move_backward_at_speed(VELOCITY);
+    shephard.chassis.move_backward(15, VELOCITY);
   } else if (timeInState > 4000 && timeInState < 10000) {
-    shephard.chassis.turn_left_at_speed(VELOCITY);
+    shephard.chassis.turn_ccw(360, VELOCITY);
   } else if (timeInState > 10000) {
     changeStateTo(STATE_NAV_LOAD);
   } 
@@ -127,22 +134,22 @@ void handleNavTargetState(void){
       break;
     case ZONE_3:
       // turn 90 degrees and then line follow
-      if (t > 1000 && t < 1000+Right_TURN_TIME) {
-        shephard.chassis.turn_right(200);
-      } else {
+      if (t > 300 && t < 300+300) {
+        makeFirstTurn();
+      } else if (t < 4000) {
         lineFollow();
       }
       break;
     case ZONE_4:
       // turn less than 90 degrees and then line follow
-      if (t < SECOUND_TURN_TIME) {
-        shephard.chassis.turn_left(200);
-      } else {
+      if (t < 100) {
+        shephard.chassis.turn_ccw(100);
+      } else if (t > 4000) {
         lineFollow();
       }
       break;
     case ZONE_TARGET:
-      if (t < 1000) {
+      if (t < 400) {
         shephard.chassis.move_forward_at_speed(VELOCITY);
       } else {
         changeStateTo(STATE_UNLOAD);
@@ -178,10 +185,10 @@ void checkForZoneChange(void) {
     } else if (right && zone == ZONE_2) {
       setFlag(flagRightLine);
       changeZoneTo(ZONE_3);
-    } else if (left && zone == ZONE_3 && curr_time - zone_time > 5000) {
+    } else if (left && zone == ZONE_3 && curr_time - zone_time > 2000) {
       changeZoneTo(ZONE_4);
       setFlag(flagLeftLine);
-    } else if (left && zone == ZONE_4 && curr_time - zone_time > 6000) {
+    } else if (left && zone == ZONE_4 && curr_time - zone_time > 2000) {
       changeZoneTo(ZONE_TARGET);
     }
 }
@@ -210,25 +217,35 @@ void lineFollow(void) {
 
     switch(line_state) {
       case STATE_ON_LINE:
-        shephard.chassis.move_forward_at_speed(VELOCITY);
+        shephard.chassis.move_forward_at_speed(130);
         break;
       case STATE_OFF_RIGHT:
-        shephard.chassis.veer_forward(250, 180);
+        shephard.chassis.veer_forward(140, 100);
         break;
       case STATE_OFF_SLIGHT_RIGHT:
-        shephard.chassis.veer_forward(250, 200);
+        shephard.chassis.veer_forward(130, 100);
         break;
       case STATE_OFF_LEFT:
-        shephard.chassis.veer_forward(180, 250);
+        shephard.chassis.veer_forward(100, 140);
         break;
       case STATE_OFF_SLIGHT_LEFT:
-        shephard.chassis.veer_forward(200, 250);
+        shephard.chassis.veer_forward(100, 130);
         break;
       default:
         Serial.println("humm");
     }
 
 }
+
+void makeFirstTurn() {
+  if (stateRed) shephard.chassis.turn_cw(90, 100);
+  else shephard.chassis.turn_ccw(90, 100);
+}
+
+void makeSecondTurn() {
+  if (stateRed) shephard.chassis.turn_ccw(70, 100);
+  else shephard.chassis.turn_ccw(70, 100);
+}
   
 uint8_t TestForKey(void) {
   uint8_t KeyEventOccurred;
diff --git a/Kabob_Code/src/StateMachine/StateMachine.h b/Kabob_Code/src/StateMachine/StateMachine.h
index 2d83038..c73b5a2 100644
--- a/Kabob_Code/src/StateMachine/StateMachine.h
+++ b/Kabob_Code/src/StateMachine/StateMachine.h
@@ -46,5 +46,7 @@ void changeLineStateTo(States_r s);
 void changeStateTo(States_t s);
 void changeZoneTo(Zones_t z);
 void setFlag(uint8_t flag);
+void makeSecondTurn(void);
+void makeFirstTurn(void);
 
 /*---------------Module Variables---------------------------*/
diff --git a/Kabob_Code/src/System.cpp b/Kabob_Code/src/System.cpp
index 8c1d017..579bbfd 100644
--- a/Kabob_Code/src/System.cpp
+++ b/Kabob_Code/src/System.cpp
@@ -2,7 +2,8 @@
 
 void System::activity(void)
 {
-    //
+    chassis.left.activity();
+    chassis.right.activity();
 }
 
 System shephard;
\ No newline at end of file
diff --git a/Kabob_Code/src/System.h b/Kabob_Code/src/System.h
index 4632697..8123da7 100644
--- a/Kabob_Code/src/System.h
+++ b/Kabob_Code/src/System.h
@@ -8,13 +8,13 @@
 
 #define RIGHT_BACKWARD_PIN 4
 #define RIGHT_FORWARD_PIN 3
-#define LEFT_BACKWARD_PIN 10
-#define LEFT_FORWARD_PIN 9
+#define LEFT_BACKWARD_PIN 9
+#define LEFT_FORWARD_PIN 10
 
-#define ENCODER_RIGHT_PIN1 8
-#define ENCODER_RIGHT_PIN2 11
-#define ENCODER_LEFT_PIN1 7
-#define ENCODER_LEFT_PIN2 5
+#define ENCODER_RIGHT_PIN1 7
+#define ENCODER_RIGHT_PIN2 8
+#define ENCODER_LEFT_PIN1 2
+#define ENCODER_LEFT_PIN2 1
 
 #define SERVO_PIN 6
 
@@ -27,11 +27,15 @@
 #define LINE_CENTER_RIGHT 16
 #define LINE_CENTER_LEFT 17
 
+#define STEPPER_DIR 16
+#define STEPPER_STEP 15
+
 #define HUB_TO_HUB_DISTANCE 25.1  // cm
 #define WHEEL_CIRCUMFERENCE 31.33 // cm
 #define MOTOR_MIN_SPEED 55        // Measured with both motors running.
                                   // If only 1 motor is running the min will be higher
 
+
 struct System
 {
 public:
@@ -65,7 +69,9 @@ struct System
                             ENCODER_LEFT_PIN2,
                             HUB_TO_HUB_DISTANCE,
                             WHEEL_CIRCUMFERENCE,
-                            MOTOR_MIN_SPEED);
+                            MOTOR_MIN_SPEED,
+                            STEPPER_DIR,
+                            STEPPER_STEP);
   Claw claw = Claw(SERVO_PIN, OPEN_CLAW_ANGLE, CLOSE_CLAW_ANGLE);
 
   void activity(void);
diff --git a/Kabob_Code/src/Test/Test.cpp b/Kabob_Code/src/Test/Test.cpp
index aafdb27..4728661 100644
--- a/Kabob_Code/src/Test/Test.cpp
+++ b/Kabob_Code/src/Test/Test.cpp
@@ -1,85 +1,31 @@
 #include "Test.h"
+#include "System.h"
+#include "Stepper.h"
 
-#define VELOCITY 255
-#define FLAG_TIME                 1000
+// change this to the number of steps on your motor
+//Stepper stepper2 = Stepper(200, 16, 15);
+// the previous reading from the analog input
+int previous = 0;
 
-unsigned long state_time;
-unsigned long serial_time; 
-unsigned long curr_time;
-unsigned long zone_time;
 
-uint8_t servoPin = 6;
-uint8_t servoPos = 0;
+void setup() {
+  // set the speed of the motor to 30 RPMs
+  //stepper2.setSpeed(60);
 
-uint8_t flagLeftLine = 0;
-uint8_t flagRightLine = 0;
-States_r line_state;
-unsigned long flag_time;
-
-
-void setup(){
-    Serial.begin(9600);
-}
-void loop(void){
-    lineFollow();
-    checkFlags();
-}   
-
-
-void checkFlags(void) {
-  if (curr_time - flag_time > FLAG_TIME) {
-    flagLeftLine = 0;
-    flagRightLine = 0;
-  }
 }
 
+void loop() {
+  // get the sensor value
 
-void lineFollow(void) {
-    uint8_t center_left = shephard.sensors.line.center_left.read();
-    uint8_t center_middle = shephard.sensors.line.center_middle.read();
-    uint8_t center_right = shephard.sensors.line.center_right.read();
-    // primary line sensing states
-    if (center_middle && !center_right && !center_left){
-      changeLineStateTo(STATE_ON_LINE);
-    }
-    else if (!center_middle && center_right && !center_left) { 
-      changeLineStateTo(STATE_OFF_LEFT);
-    }
-    else if (!center_middle && !center_right && center_left) { 
-      changeLineStateTo(STATE_OFF_RIGHT);
-    }
-    else if (center_middle && !center_right && center_left && !flagLeftLine) { 
-      changeLineStateTo(STATE_OFF_SLIGHT_RIGHT);
-    }
-    else if (center_middle && center_right && !center_left && !flagRightLine) { 
-      changeLineStateTo(STATE_OFF_SLIGHT_LEFT);
-    }
+  // move a number of steps equal to the change in the
+  // sensor reading
+  shephard.chassis.stepper.cw(100);
 
-    switch(line_state) {
-      case STATE_ON_LINE:
-        shephard.chassis.move_forward_at_speed(VELOCITY);
-        break;
-      case STATE_OFF_RIGHT:
-        shephard.chassis.veer_forward(250, 180);
-        break;
-      case STATE_OFF_SLIGHT_RIGHT:
-        shephard.chassis.veer_forward(250, 200);
-        break;
-      case STATE_OFF_LEFT:
-        shephard.chassis.veer_forward(180, 250);
-        break;
-      case STATE_OFF_SLIGHT_LEFT:
-        shephard.chassis.veer_forward(200, 250);
-        break;
-      default:
-        Serial.println("humm");
-    }
+  //stepper2.step(100);
+  delay(1000);
 
-}
+  Serial.println("test");
 
 
-void changeLineStateTo(States_r s) {
-  if (s != line_state) {
-    line_state = s;
-  }
+  // remember the previous value of the sensor
 }
\ No newline at end of file
diff --git a/Kabob_Code/src/Test/Test.h b/Kabob_Code/src/Test/Test.h
index db19715..25a4b15 100644
--- a/Kabob_Code/src/Test/Test.h
+++ b/Kabob_Code/src/Test/Test.h
@@ -1,45 +1,4 @@
 #pragma once
 
 #include <Arduino.h>
-#include <Wire.h>
-#include "System.h"
-
-
-
-typedef enum {
-  STATE_IDLE, STATE_LOAD, STATE_NAV_TARGET, STATE_UNLOAD, 
-  STATE_NAV_LOAD,  
-} States_t;
-
-typedef enum {
-  STATE_ON_LINE, STATE_OFF_LEFT, STATE_OFF_RIGHT,
-  STATE_OFF_SLIGHT_RIGHT, STATE_OFF_SLIGHT_LEFT
-} States_r;
-
-// Zone defintions
-typedef enum {
-  ZONE_LOAD, ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_TARGET
-} Zones_t;
-
-
-void checkGlobalEvents(void);
-void checkForZoneChange(void);
-void lineFollow(void);
-void handleMoveForward(void);
-void handleMoveBackward(void);
-uint8_t TestForKey(void);
-void RespToKey(void);
-void handleRightTurn(void);
-void handleLeftTurn(void);
-void handleLoadState(void);
-void handleUnloadState(void);
-void handleNavTargetState(void);
-void handleNavLoadState(void);
-void changeStateTo(States_t);
-void setup(void);
-void loop(void);
-void checkFlags(void);
-void changeLineStateTo(States_r s);
-void changeStateTo(States_t s);
-void changeZoneTo(Zones_t z);
-void setFlag(uint8_t flag);
\ No newline at end of file
+#include <Wire.h>
\ No newline at end of file
diff --git a/Kabob_Code/src/Test/code_snippets.md b/Kabob_Code/src/Test/code_snippets.md
index 0fb0995..e2a05d3 100644
--- a/Kabob_Code/src/Test/code_snippets.md
+++ b/Kabob_Code/src/Test/code_snippets.md
@@ -162,3 +162,25 @@ void loop(void){
     }
 }
 ```
+
+### Encoder Funcionality
+
+```c++
+unsigned long start; 
+void setup(){
+    Serial.begin(9600);
+    while(!Serial);
+    Serial.println("Starting");
+    start = millis();
+}
+
+void loop(void){
+  Serial.println(millis() - start);
+  if (millis() - start < 500) {
+    Serial.println("test");
+    shephard.chassis.move_forward(20, 150);
+  }
+  shephard.activity();
+  delay(100);
+}   
+```
\ No newline at end of file