diff --git a/firmware/dc_motor_controller/dc_motor_controller.ino b/firmware/dc_motor_controller/dc_motor_controller.ino
index 9579378..c3b8723 100644
--- a/firmware/dc_motor_controller/dc_motor_controller.ino
+++ b/firmware/dc_motor_controller/dc_motor_controller.ino
@@ -1,5 +1,6 @@
 #include "SoftI2CMaster.h"
 #include <Wire.h>
+//#include <avr/wdt.h>
 
 /* H-bridge defines */
 #define PWM1M1 5
@@ -7,8 +8,9 @@
 #define PWM1M2 9
 #define PWM2M2 10
 /* Limits for control signal */
-#define outMax 127
-#define outMin 0
+#define outMax 120
+#define outMin 35
+#define Deadband 1
 /* Encoder defines */
 #define sda_pin 16
 #define scl_pin 14
@@ -19,20 +21,24 @@
 /* Ratio of worm gear */
 #define RATIO 30
 /* Maximum Angle for homing scanning */
-#define MAX_AZ_ANGLE 110
-#define MAX_EL_ANGLE 370
+#define SEEK_MOVE 30
+#define MIN_AZ_ANGLE 0
+#define MAX_AZ_ANGLE 370
+#define MIN_EL_ANGLE 0
+#define MAX_EL_ANGLE 110
 /* Homing switch */
 #define HOME_AZ 4
 #define HOME_EL 7
 /* Change to LOW according to Home sensor */
 #define DEFAULT_HOME_STATE 1
 /* Time to disable the motors in millisecond */
-#define T_DELAY 60000
+#define T_DELAY 1000
 /* PIN for Enable or Disable Stepper Motors */
 #define EN 8
 /* Serial configuration */
 #define BufferSize 256
 #define BaudRate 19200
+#define TX_EN A3
 /*Global Variables*/
 unsigned long t_DIS = 0; /*time to disable the Motors*/
 /* angle offset */
@@ -51,120 +57,167 @@ void setup()
   /* Encoder */
   i2c_soft.begin();
   Wire.begin();
+  /* Enable Motors */
+  pinMode(EN, OUTPUT);
+  digitalWrite(EN, HIGH);
   /* Homing switch */
   pinMode(HOME_AZ, INPUT_PULLUP);
   pinMode(HOME_EL, INPUT_PULLUP);
   /* Serial Communication */
   Serial.begin(BaudRate);
-  /* Enable Motors */
-  pinMode(EN, OUTPUT);
-  digitalWrite(EN, HIGH);
   /* RS485 */
-  //Serial1.begin(9600);
-  //pinMode(A3, OUTPUT);
-  //digitalWrite(A3, HIGH);
+  //Serial1.begin(BaudRate);
+  pinMode(TX_EN, OUTPUT);
+  digitalWrite(TX_EN, LOW);
+  /* WDT */
+  //cli();
+  //WDTCSR |= (1<<WDCE) | (1<<WDE);
+  //WDTCSR = (0<<WDIE) | (1<<WDE) | (0<<WDP3) | (1<<WDP2) | (1<<WDP1) | (0<<WDP0);
+  //sei();
   /* Initial Homing */
-  Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, true);
+  //Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, true);
 }
 
 void loop()
 {
   double *set_point;
+  double curr_pos[2];
+
   /* Read commands from serial */
   set_point = cmd_proc();
+  //set_point = cmd_proc_RS485();
   /* Move Motor */
   dc_move(set_point);
+  /* Reset WDT */
+  //wdt_reset();
+  /* Time Check */
+  if (t_DIS == 0)
+    t_DIS = millis();
+  /* Disable Motors */
+  get_position(curr_pos);
+  if (abs(curr_pos[0]-set_point[0]) < Deadband && abs(curr_pos[1]-set_point[1]) < Deadband && millis()-t_DIS > T_DELAY)
+    digitalWrite(EN, LOW);
+  /* Enable Motors */
+  else
+    digitalWrite(EN, HIGH);
 }
 
-/* EasyComm 2 Protocol */
-double * cmd_proc()
-{
-  static double set_point[] = {0, 0};
-  /* Serial */
-  char buffer[BufferSize];
-  char incomingByte;
-  char *Data = buffer;
-  char *rawData;
-  static int BufferCnt = 0;
-  char data[100];
-  /* Read from serial */
-  while (Serial.available() > 0)
-  {
-    incomingByte = Serial.read();
-    /* New data */
-    if (incomingByte == '\n')
-    {
-      buffer[BufferCnt] = 0;
-      if (buffer[0] == 'A' && buffer[1] == 'Z')
-      {
-        if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L')
-        {
-          double *pos;
-          pos = get_position();
-          /* Get position */
-          Serial.print("AZ");
-          Serial.print(pos[0], 1);
-          Serial.print(" ");
-          Serial.print("EL");
-          Serial.println(pos[1], 1);
-        }
-        else
-        {
-          /* Get the absolute value of angle */
-          rawData = strtok_r(Data, " " , &Data);
-          strncpy(data, rawData + 2, 10);
-          if (isNumber(data))
-            set_point[0] = atof(data);
-          /* Get the absolute value of angle */
-          rawData = strtok_r(Data, " " , &Data);
-          if (rawData[0] == 'E' && rawData[1] == 'L')
-          {
-            strncpy(data, rawData + 2, 10);
-            if (isNumber(data))
-              set_point[1] = atof(data);
-          }
-        }
-      }
-      /* Stop Moving */
-      else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E')
-      {
-        double *pos;
-        pos = get_position();
-        /* Get position */
-        Serial.print("AZ");
-        Serial.print(pos[0], 1);
-        Serial.print(" ");
-        Serial.print("EL");
-        Serial.println(pos[1], 1);
-        set_point[0] = pos[0];
-        set_point[1] = pos[1];
-      }
-      /* Reset the rotator */
-      else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T')
-      {
-        double *pos;
-        pos = get_position();
-        /* Get position */
-        Serial.print("AZ");
-        Serial.print(pos[0], 1);
-        Serial.print(" ");
-        Serial.print("EL");
-        Serial.println(pos[1], 1);
-        /* Move to initial position */
-        Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, false);
-        set_point[0] = 0;
-        set_point[1] = 0;
-      }
-      BufferCnt = 0;
-    }
-    /* Fill the buffer with incoming data */
-    else {
-      buffer[BufferCnt] = incomingByte;
-      BufferCnt++;
-    }
-  }
-  return set_point;
-}
+///* EasyComm 2 Protocol for RS485 */
+//double * cmd_proc_RS485()
+//{
+//  static double set_point[] = {0, 0};
+//  /* Serial */
+//  static char buffer[BufferSize];
+//  char incomingByte;
+//  char *Data = buffer;
+//  char *rawData;
+//  static int BufferCnt = 0;
+//  char data[100];
+//  double pos[2];
+//
+//  /* Read from serial */
+//  while (Serial1.available() > 0)
+//  {
+//    incomingByte = Serial1.read();
+//    /* New data */
+//    if (incomingByte == '\n')
+//    {
+//      buffer[BufferCnt] = 0;
+//      if (buffer[0] == 'A' && buffer[1] == 'Z')
+//      {
+//        if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L')
+//        {
+//          get_position(pos);
+//          /* Get position */
+//          digitalWrite(TX_EN, HIGH);
+//          delay(8);
+//          Serial1.print("AZ");
+//          Serial1.print(pos[0], 1);
+//          Serial1.print(" ");
+//          Serial1.print("EL");
+//          Serial1.println(pos[1], 1);
+//          delay(8);
+//          digitalWrite(TX_EN, LOW);
+//        }
+//        else
+//        {
+//          /* Get the absolute value of angle */
+//          rawData = strtok_r(Data, " " , &Data);
+//          strncpy(data, rawData + 2, 10);
+//          if (isNumber(data))
+//          {
+//            set_point[0] = atof(data);
+//            if (set_point[0] > MAX_AZ_ANGLE)
+//              set_point[0] = MAX_AZ_ANGLE;
+//            else if (set_point[0] < MIN_AZ_ANGLE)
+//              set_point[0] = MIN_AZ_ANGLE;
+//          }
+//          /* Get the absolute value of angle */
+//          rawData = strtok_r(Data, " " , &Data);
+//          if (rawData[0] == 'E' && rawData[1] == 'L')
+//          {
+//            strncpy(data, rawData + 2, 10);
+//            if (isNumber(data))
+//            {
+//              set_point[1] = atof(data);
+//              if (set_point[1] > MAX_EL_ANGLE)
+//                set_point[1] = MAX_EL_ANGLE;
+//              else if (set_point[1] < MIN_EL_ANGLE)
+//                set_point[1] = MIN_EL_ANGLE;
+//            }
+//          }
+//        }
+//      }
+//      /* Stop Moving */
+//      else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E')
+//      {
+//        get_position(pos);
+//        /* Get position */
+//        digitalWrite(TX_EN, HIGH);
+//        delay(8);
+//        Serial1.print("AZ");
+//        Serial1.print(pos[0], 1);
+//        Serial1.print(" ");
+//        Serial1.print("EL");
+//        Serial1.println(pos[1], 1);
+//        delay(8);
+//        digitalWrite(TX_EN, LOW);
+//        set_point[0] = pos[0];
+//        set_point[1] = pos[1];
+//      }
+//      /* Reset the rotator */
+//      else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T')
+//      {
+//        get_position(pos);
+//        /* Get position */
+//        digitalWrite(TX_EN, HIGH);
+//        delay(8);
+//        Serial1.print("AZ");
+//        Serial1.print(pos[0], 1);
+//        Serial1.print(" ");
+//        Serial1.print("EL");
+//        Serial1.println(pos[1], 1);
+//        delay(8);
+//        digitalWrite(TX_EN, LOW);
+//        /* Move to initial position */
+//        Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, false);
+//        set_point[0] = 0;
+//        set_point[1] = 0;
+//      }
+//      /* Clear serial buffer */
+//      BufferCnt = 0;
+//      Serial1.read();
+//      /* Reset the disable motor time */
+//      t_DIS = 0;
+//    }
+//    /* Fill the buffer with incoming data */
+//    else {
+//      buffer[BufferCnt] = incomingByte;
+//      BufferCnt++;
+//    }
+//  }
+//  return set_point;
+//}
 
 /* Homing Function */
 void Homing(double AZangle, double ELangle, bool Init)
@@ -173,69 +226,76 @@ void Homing(double AZangle, double ELangle, bool Init)
   int value_Home_EL = DEFAULT_HOME_STATE;
   boolean isHome_AZ = false;
   boolean isHome_EL = false;
-  double *zero_angle;
-  double *curr_angle;
-  double set_point[] = {0, 0};
+  double zero_angle[2];
+  double curr_angle[2];
+  double set_point[2];
 
-  zero_angle = get_position();
-
-  if (zero_angle[0] > 0)
-    set_point[0] = AZangle;
+  get_position(zero_angle);
+  if (Init == true)
+  {
+    set_point[0] = zero_angle[0]+SEEK_MOVE;
+    set_point[1] = zero_angle[1]+SEEK_MOVE;
+  }
   else
-    set_point[0] = -AZangle;
-
-  if (zero_angle[1] > 0)
-    set_point[1] = ELangle;
-  else
-    set_point[1] = -ELangle;
-
+  {
+    set_point[0] = 10;//zero_angle[0]-MAX_AZ_ANGLE;
+    set_point[1] = 10;//zero_angle[1]-MAX_EL_ANGLE;
+  }
   while (isHome_AZ == false || isHome_EL == false)
   {
     dc_move(set_point);
+    delay(1);
     value_Home_AZ = digitalRead(HOME_AZ);
+    delay(1);
     value_Home_EL = digitalRead(HOME_EL);
-
     /* Change to LOW according to Home sensor */
-    if (value_Home_AZ == DEFAULT_HOME_STATE)
+    if (value_Home_AZ == DEFAULT_HOME_STATE && isHome_AZ == false)
     {
       isHome_AZ = true;
-      curr_angle = get_position();
+      get_position(curr_angle);
       set_point[0] = 0;
       if (Init)
         az_angle_offset = curr_angle[0];
     }
-    if (value_Home_EL == DEFAULT_HOME_STATE)
+    if (value_Home_EL == DEFAULT_HOME_STATE && isHome_EL == false)
     {
       isHome_EL = true;
-      curr_angle = get_position();
+      get_position(curr_angle);
       set_point[1] = 0;
       if (Init)
         el_angle_offset = curr_angle[1];
     }
-    curr_angle = get_position();
-    if ((abs(curr_angle[0] - zero_angle[0]) > MAX_AZ_ANGLE && !isHome_AZ) || (abs(curr_angle[1] - zero_angle[1]) > MAX_EL_ANGLE && !isHome_EL))
+    get_position(curr_angle);
+    if (abs(curr_angle[0] - zero_angle[0]) >= SEEK_MOVE && !isHome_AZ && Init)
+      set_point[0] = -MAX_AZ_ANGLE;
+    if (abs(curr_angle[1] - zero_angle[1]) >= SEEK_MOVE && !isHome_EL && Init)
+      set_point[1] = -MAX_EL_ANGLE;
+    if ((abs(curr_angle[0] - zero_angle[0]) >= MAX_AZ_ANGLE && !isHome_AZ) || (abs(curr_angle[1] - zero_angle[1]) >= MAX_EL_ANGLE && !isHome_EL))
     {
       /* set error */
-      break;
+      while(1)
+      {
+        ;
+      }
     }
   }
 }
 
 void dc_move(double set_point[])
 {
-  static double u[] = {0, 0};
-  static double *curr_pos;
+  double u[2];
+  double curr_pos[2];
   static double prev_pos[] = {0, 0};
-  static double error[] = {0, 0};
-  static double Iterm[] = {0, 0};
-  static double Pterm[] = {0, 0};
-  static double Dterm[] = {0, 0};
+  double error[2];
+  double Iterm[2];
+  double Pterm[2];
+  double Dterm[2];
   double Kp = 200;
-  double Ki = 1;
+  double Ki = 2;
   double Kd = 0;
   double dt = 0.001; // calculate
 
-  curr_pos = get_position();
+  get_position(curr_pos);
   error[0] = set_point[0] - curr_pos[0];
   error[1] = set_point[1] - curr_pos[1];
 
@@ -291,7 +351,7 @@ void dc_move(double set_point[])
 }
 
 /* Read Encoders */
-double * get_position()
+void get_position(double *new_pos)
 {
   int low_raw, high_raw, status_val;
   double raw_pos = 0;
@@ -388,7 +448,111 @@ double * get_position()
   else
     ; /* set error */
 
-  return real_pos;
+  new_pos[0] = real_pos[0];
+  new_pos[1] = real_pos[1];
+}
+
+/* EasyComm 2 Protocol */
+double * cmd_proc()
+{
+  static double set_point[] = {0, 0};
+  /* Serial */
+  char buffer[BufferSize];
+  char incomingByte;
+  char *Data = buffer;
+  char *rawData;
+  static int BufferCnt = 0;
+  char data[100];
+  double pos[2];
+
+  /* Read from serial */
+  while (Serial.available() > 0)
+  {
+    incomingByte = Serial.read();
+    /* New data */
+    if (incomingByte == '\n')
+    {
+      buffer[BufferCnt] = 0;
+      if (buffer[0] == 'A' && buffer[1] == 'Z')
+      {
+        if (buffer[2] == ' ' && buffer[3] == 'E' && buffer[4] == 'L')
+        {
+          get_position(pos);
+          /* Get position */
+          Serial.print("AZ");
+          Serial.print(pos[0], 1);
+          Serial.print(" ");
+          Serial.print("EL");
+          Serial.println(pos[1], 1);
+        }
+        else
+        {
+          /* Get the absolute value of angle */
+          rawData = strtok_r(Data, " " , &Data);
+          strncpy(data, rawData + 2, 10);
+          if (isNumber(data))
+          {
+            set_point[0] = atof(data);
+            if (set_point[0] > MAX_AZ_ANGLE)
+              set_point[0] = MAX_AZ_ANGLE;
+            else if (set_point[0] < MIN_AZ_ANGLE)
+              set_point[0] = MIN_AZ_ANGLE;
+          }
+          /* Get the absolute value of angle */
+          rawData = strtok_r(Data, " " , &Data);
+          if (rawData[0] == 'E' && rawData[1] == 'L')
+          {
+            strncpy(data, rawData + 2, 10);
+            if (isNumber(data))
+            {
+              set_point[1] = atof(data);
+              if (set_point[1] > MAX_EL_ANGLE)
+                set_point[1] = MAX_EL_ANGLE;
+              else if (set_point[1] < MIN_EL_ANGLE)
+                set_point[1] = MIN_EL_ANGLE;
+            }
+          }
+        }
+      }
+      /* Stop Moving */
+      else if (buffer[0] == 'S' && buffer[1] == 'A' && buffer[2] == ' ' && buffer[3] == 'S' && buffer[4] == 'E')
+      {
+        get_position(pos);
+        /* Get position */
+        Serial.print("AZ");
+        Serial.print(pos[0], 1);
+        Serial.print(" ");
+        Serial.print("EL");
+        Serial.println(pos[1], 1);
+        set_point[0] = pos[0];
+        set_point[1] = pos[1];
+      }
+      /* Reset the rotator */
+      else if (buffer[0] == 'R' && buffer[1] == 'E' && buffer[2] == 'S' && buffer[3] == 'E' && buffer[4] == 'T')
+      {
+        get_position(pos);
+        /* Get position */
+        Serial.print("AZ");
+        Serial.print(pos[0], 1);
+        Serial.print(" ");
+        Serial.print("EL");
+        Serial.println(pos[1], 1);
+        /* Move to initial position */
+        Homing(-MAX_AZ_ANGLE, -MAX_EL_ANGLE, false);
+        set_point[0] = 0;
+        set_point[1] = 0;
+      }
+      BufferCnt = 0;
+      /* Reset the disable motor time */
+      t_DIS = 0;
+    }
+    /* Fill the buffer with incoming data */
+    else {
+      buffer[BufferCnt] = incomingByte;
+      BufferCnt++;
+    }
+  }
+  return set_point;
 }
 
 /* check if is argument in number */