diff --git a/Kabob_Code/src/Sensors/LineSensor.h b/Kabob_Code/src/Sensors/LineSensor.h
index 4c13cae..3b7db31 100644
--- a/Kabob_Code/src/Sensors/LineSensor.h
+++ b/Kabob_Code/src/Sensors/LineSensor.h
@@ -9,7 +9,7 @@ class LineSensor
     int read();
     int readAnalog();     //adding for debug purposes
     int getPin();
-    const int lineThreshold = 600; 
+    const int lineThreshold = 400; 
   private:
     int pin;
 };
\ No newline at end of file
diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp
index 30704dc..d97b681 100644
--- a/Kabob_Code/src/StateMachine/StateMachine.cpp
+++ b/Kabob_Code/src/StateMachine/StateMachine.cpp
@@ -1,29 +1,45 @@
 #include "StateMachine.h"
 
+#define VELOCITY 200
+
 States_t state;
+States_r line_state;
+Zones_t zone;
 
 unsigned long state_time;
 unsigned long serial_time; 
-uint8_t servoPin = 6;
-uint8_t servoPos = 0;
+unsigned long curr_time;
+unsigned long zone_time;
+
+uint8_t flagLeftLine = 0;
+uint8_t flagRightLine = 0;
+
+uint8_t DEBUG = false;
+
+unsigned long flag_time;
 
 void setup() {
   // put your setup code here, to run once:
   Serial.begin(9600);
   while(!Serial);
+  //delay(10000);
 
-  //timer 
+  //times
+  curr_time = millis();
   serial_time = millis();
   state_time = millis();
+  flag_time = millis();
 
-  state = STATE_IDLE;
+  changeStateTo(STATE_LOAD);
   
   Serial.println("Setup Complete!");
 }
 
 void loop() {
+  curr_time = millis();
   checkGlobalEvents();
-  shephard.activity();
+  checkFlags();
+  checkForZoneChange();
 
   switch(state) {
     case STATE_IDLE:
@@ -36,54 +52,210 @@ void loop() {
       handleNavTargetState();
       break;
     case STATE_UNLOAD:
-      shephard.chassis.move_backward_at_speed(100);
+      handleUnloadState();
       break;
     case STATE_NAV_LOAD:
+      handleNavLoadState();
       break;
     default:
       Serial.println("What is this I do not even...");
   }
 
-  unsigned long current_millis = millis();
-  if (current_millis - serial_time > MILLISECONDS(1/PRINT_FREQUENCY) ){
-    Serial.println("One Secound update: ");
-    Serial.println("left " + String(shephard.sensors.line.left.readAnalog()));
-    Serial.println("right " + String(shephard.sensors.line.right.readAnalog()));
-    Serial.println("center left " + String(shephard.sensors.line.center_left.readAnalog()));
-    Serial.println("center middle " + String(shephard.sensors.line.center_middle.readAnalog()));
-    Serial.println("center right " + String(shephard.sensors.line.center_right.readAnalog()));
-    serial_time = current_millis;
+  //debug for line sensors
+  if (DEBUG && curr_time - serial_time > MILLISECONDS(1/PRINT_FREQUENCY) ){
+    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;
   }
-
 }
 
 void handleLoadState(void) {
-  int timeInState = millis() - state_time;
-  if (timeInState < 1500) {
+  unsigned int timeInState = curr_time - state_time;
+  if (timeInState < 1000) {
     shephard.claw.open(); 
-  } else if (timeInState > 3000 && timeInState < 5000) {
+  } else if (timeInState > 1000 && timeInState < 2000) {
     shephard.claw.close(); 
-  } else if (timeInState > 5000) {
+  } else if (timeInState > 2000  && timeInState < 3000) {
+    // THIS IS WHERE THE ROBOT GETS HOSED
+    // cannot go straight for more than 1 or 2 secounds
+    shephard.chassis.move_forward_at_speed(VELOCITY);
+  } else if (timeInState > 3000) { 
     changeStateTo(STATE_NAV_TARGET);
+    changeZoneTo(ZONE_A);
   }
 }
 
+void handleUnloadState(void) {
+  int timeInState = curr_time - state_time;
+  if (timeInState < 1000) {
+    shephard.claw.open(); 
+  } 
+  
+  // back up after depositing ball
+  else if (timeInState > 1000 && timeInState < 4000) {
+    shephard.chassis.move_backward_at_speed(VELOCITY);
+  } 
+  // about 6 secounds to rotate 180 degrees
+  else if (timeInState > 4000 && timeInState < 10000) {
+    shephard.chassis.turn_right_at_speed(255);
+  } 
+  
+  else if (timeInState > 9000) {
+    changeStateTo(STATE_NAV_LOAD);
+  } 
+}
+
 void handleNavTargetState(void){
-  shephard.chassis.move_forward_at_speed(200);
-  if (millis() - state_time> 5000) {
-    changeStateTo(STATE_IDLE);
-  }       
+  unsigned long t = curr_time - zone_time;   
+  switch(zone) {
+    case ZONE_LOAD:
+    break;
+    case ZONE_A:
+      shephard.chassis.move_forward_at_speed(200);
+      break;
+    case ZONE_B:
+      if (t < 3000) {
+        shephard.chassis.turn_left(200);
+      } else {
+        lineFollow();
+      }
+      break;
+    case ZONE_C:
+      if (t < 3000) {
+        shephard.chassis.turn_left(200);
+      } else {
+        lineFollow();
+      }
+      break;
+    case ZONE_1:
+      if (t < 4000) {
+        shephard.chassis.move_forward_at_speed(200);
+      } else {
+        lineFollow();
+      }
+      break;
+    case ZONE_2:
+      lineFollow();
+      break;
+    case ZONE_3:
+      // turn 90 degrees and then line follow
+      if (t > 1000 && t < 5000) {
+        shephard.chassis.turn_right(200);
+      } else {
+        lineFollow();
+      }
+      break;
+    case ZONE_4:
+      // turn 90 degrees and then line follow
+      if (t < 3000) {
+        shephard.chassis.turn_left(200);
+      } else {
+        lineFollow();
+      }
+      break;
+    case ZONE_TARGET:
+      if (t < 1000) {
+        shephard.chassis.move_forward_at_speed(VELOCITY);
+      } else {
+        changeStateTo(STATE_UNLOAD);
+      }
+      break;
+    default:
+      Serial.println("Zone has more states than Paxton thought");
+  }
 }
 
-void changeStateTo(States_t s) {
-  state = s;
-  state_time = millis();
-  Serial.println("New state = " + s);
+void handleNavLoadState(void) {
+  shephard.chassis.stop();
 }
 
 void checkGlobalEvents(void) {
   if (TestForKey()) RespToKey();
 }
+
+void checkFlags(void) {
+  if (curr_time - flag_time > FLAG_TIME) {
+    flagLeftLine = 0;
+    flagRightLine = 0;
+  }
+}
+
+void checkForZoneChange(void) {
+    uint8_t left = shephard.sensors.line.left.read();
+    uint8_t right = shephard.sensors.line.right.read();
+    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();
+    // if (left) Serial.println("left sensor tripped");
+    Serial.println(zone);
+    if (left && (zone == ZONE_A)) {
+      changeZoneTo(ZONE_B);
+    } else if (left && zone == ZONE_B && curr_time - zone_time > 3000) {
+      changeZoneTo(ZONE_C);
+    } else if (right && zone == ZONE_C && curr_time - zone_time > 5000) {
+      changeZoneTo(ZONE_1);
+    } else if (left && zone == ZONE_1) {
+      setFlag(flagLeftLine);
+      changeZoneTo(ZONE_2);
+    } else if (right && zone == ZONE_2) {
+      setFlag(flagRightLine);
+      changeZoneTo(ZONE_3);
+    } else if (left && zone == ZONE_3 && curr_time - zone_time > 5000) {
+      changeZoneTo(ZONE_4);
+      setFlag(flagLeftLine);
+    } else if (left && zone == ZONE_4 && curr_time - zone_time > 6000) {
+      changeZoneTo(ZONE_TARGET);
+    }
+}
+
+
+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);
+    }
+
+    switch(line_state) {
+      case STATE_ON_LINE:
+        shephard.chassis.move_forward_at_speed(250);
+        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");
+    }
+
+}
   
 uint8_t TestForKey(void) {
   uint8_t KeyEventOccurred;
@@ -91,6 +263,33 @@ uint8_t TestForKey(void) {
   return KeyEventOccurred;
 }
 
+
+void changeStateTo(States_t s) {
+  if (s != state) {
+    state = s;
+    state_time = curr_time;
+  }
+}
+
+void changeLineStateTo(States_r s) {
+  if (s != line_state) {
+    line_state = s;
+  }
+}
+
+void changeZoneTo(Zones_t z) {
+  if (z != zone) {
+    zone = z;
+    zone_time = millis();
+    Serial.println("In new zone " + z);
+  }
+}
+
+void setFlag(uint8_t flag){
+  flag = true;
+  flag_time = curr_time;
+}
+
 void RespToKey(void) {
   uint8_t theKey;
   theKey = Serial.read();
@@ -102,7 +301,13 @@ void RespToKey(void) {
     case ' ': 
       state = STATE_IDLE;
       break;
+    case 'r': 
+      changeStateTo(STATE_NAV_TARGET);      
+      break;
     default:
       break;
   }
 }
+
+
+
diff --git a/Kabob_Code/src/StateMachine/StateMachine.h b/Kabob_Code/src/StateMachine/StateMachine.h
index 006e6d2..a5f6b35 100644
--- a/Kabob_Code/src/StateMachine/StateMachine.h
+++ b/Kabob_Code/src/StateMachine/StateMachine.h
@@ -6,14 +6,28 @@
 
 #define PRINT_FREQUENCY                 1 // hz
 #define MILLISECONDS(time)({time * 1000;})
+#define FLAG_TIME                 1000
 
 /*---------------State Definitions--------------------------*/
 typedef enum {
-  STATE_IDLE, STATE_LOAD, STATE_NAV_TARGET, STATE_UNLOAD, STATE_NAV_LOAD
+  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_A, ZONE_B, ZONE_C, ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_TARGET
+} Zones_t;
+
 /*----------------------------Function Prototypes------------*/
 void checkGlobalEvents(void);
+void checkForZoneChange(void);
+void lineFollow(void);
 void handleMoveForward(void);
 void handleMoveBackward(void);
 uint8_t TestForKey(void);
@@ -21,9 +35,16 @@ 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);
 
 /*---------------Module Variables---------------------------*/
diff --git a/Kabob_Code/src/Test/Test.cpp b/Kabob_Code/src/Test/Test.cpp
index de8b1cd..c27f9ce 100644
--- a/Kabob_Code/src/Test/Test.cpp
+++ b/Kabob_Code/src/Test/Test.cpp
@@ -4,6 +4,9 @@ void setup(){
     Serial.begin(9600);
 }
 void loop(void){
-    Serial.println("Currently doing some testing...");
-    delay(1000);
+    shephard.chassis.turn_left_at_speed(255);
+    delay(6000);
+    shephard.chassis.stop();
+    delay(2000);
+
 }   
\ No newline at end of file
diff --git a/Kabob_Code/src/Test/Test.h b/Kabob_Code/src/Test/Test.h
index 4ab92df..141a578 100644
--- a/Kabob_Code/src/Test/Test.h
+++ b/Kabob_Code/src/Test/Test.h
@@ -2,3 +2,4 @@
 
 #include <Arduino.h>
 #include <Wire.h>
+#include "System.h"
\ No newline at end of file