diff --git a/Arduino/SatNOGS.ino b/Arduino/SatNOGS.ino
index 150917e..91e99ca 100644
--- a/Arduino/SatNOGS.ino
+++ b/Arduino/SatNOGS.ino
@@ -3,17 +3,26 @@
 #include <math.h>
 #include <AccelStepper.h>
 
-#define DIR_AZ 18
-#define STEP_AZ 10
-#define DIR_EL 6
-#define STEP_EL 7
+#define DIR_AZ 18 //PIN for Azimuth Direction
+#define STEP_AZ 10 //PIN for Azimuth Steps
+#define DIR_EL 6 //PIN for Elevation Direction
+#define STEP_EL 7 //PIN for Elevation Steps
 
-#define MS1 9
-#define EN 8
+#define MS1 9 //PIN for step size
+#define EN 8 //PIN for Enable or Disable Stepper Motors
 
 #define SPR 200 //Step Per Revolution
 #define RATIO 60 //Gear ratio
-#define T_DEALY 20000 //Time to disable the motors
+#define T_DELAY 20000 //Time to disable the motors in millisecond
+
+#define HOME_AZ 4 //Homing switch for Azimuth
+#define HOME_EL 5 //Homing switch for Elevation
+/*The MAX_ANGLE depends of ANGLE_SCANNING_MULT and maybe misbehave for large values*/
+#define ANGLE_SCANNING_MULT 10 //Angle scanning multiplier
+#define MAX_AZ_ANGLE 200 //Maximum Angle of Azimuth for homing scanning
+#define MAX_EL_ANGLE 100 //Maximum Angle of Elevation for homing scanning
+
+#define HOME_DELAY 6000 //Time for homing Decceleration in millisecond
 
 /*Global Variables*/
 unsigned long t_DIS = 0; //time to disable the Motors
@@ -25,12 +34,12 @@ AccelStepper ELstepper(1, STEP_EL, DIR_EL);
 void setup()
 {  
   /*Change these to suit your stepper if you want*/
-  AZstepper.setMaxSpeed(400);
-  AZstepper.setAcceleration(100);
+  AZstepper.setMaxSpeed(200);
+  AZstepper.setAcceleration(200);
   
   /*Change these to suit your stepper if you want*/
   ELstepper.setMaxSpeed(400);
-  ELstepper.setAcceleration(100);
+  ELstepper.setAcceleration(200);
   
   /*Enable Motors*/
   pinMode(EN, OUTPUT);
@@ -38,8 +47,13 @@ void setup()
   /*Step size*/
   pinMode(MS1, OUTPUT);
   digitalWrite(MS1, LOW); //Full step
+  /*Homing switch*/
+  pinMode(HOME_AZ, INPUT);
+  pinMode(HOME_EL, INPUT);
   /*Serial Communication*/
   Serial.begin(19200);
+  /*Initial Homing*/
+  Initial_Homing();
 }
 
 void loop()
@@ -53,7 +67,7 @@ void loop()
     t_DIS = millis();
   
   /*Disable Motors*/
-  if (AZstep == AZstepper.currentPosition() && ELstep == ELstepper.currentPosition() && millis()-t_DIS > T_DEALY)
+  if (AZstep == AZstepper.currentPosition() && ELstep == ELstepper.currentPosition() && millis()-t_DIS > T_DELAY)
     digitalWrite(EN, HIGH);
   /*Enable Motors*/
   else
@@ -65,6 +79,73 @@ void loop()
   stepper_move(AZstep, ELstep);
 }
 
+void Initial_Homing()
+{
+  int value_Home_AZ = HIGH;
+  int value_Home_EL = HIGH;
+  int n_AZ = 1; //Times that AZ angle has changed
+  int n_EL = 1; //Times that EL angle has changed
+  int AZsteps = deg2step(-n_AZ*ANGLE_SCANNING_MULT); //Initial scanning steps for AZ 
+  int ELsteps = deg2step(-n_EL*ANGLE_SCANNING_MULT); //Initial scanning steps for EL
+  boolean isHome_AZ = false;
+  boolean isHome_EL = false;
+  
+  AZstepper.moveTo(AZsteps);
+  ELstepper.moveTo(ELsteps);
+  
+  while(isHome_AZ == false || isHome_EL == false)
+  {
+    value_Home_AZ = digitalRead(HOME_AZ);
+    value_Home_EL = digitalRead(HOME_EL);
+
+    if (value_Home_AZ == LOW)
+    {
+      AZstepper.moveTo(AZstepper.currentPosition());
+      isHome_AZ = true;
+    }   
+    if (value_Home_EL == LOW)
+    {
+      ELstepper.moveTo(ELstepper.currentPosition());
+      isHome_EL = true;
+    }
+    if (AZstepper.distanceToGo() == 0 && !isHome_AZ)
+    {
+      n_AZ++;
+      AZsteps = deg2step(pow(-1,n_AZ)*n_AZ*ANGLE_SCANNING_MULT);
+      if (abs(n_AZ*ANGLE_SCANNING_MULT) > MAX_AZ_ANGLE)
+      {
+        error(0);
+        break;
+      }
+      AZstepper.moveTo(AZsteps);
+    } 
+    if (ELstepper.distanceToGo() == 0 && !isHome_EL)
+    { 
+      n_EL++;
+      ELsteps = deg2step(pow(-1,n_EL)*n_EL*ANGLE_SCANNING_MULT);
+      if (abs(n_EL*ANGLE_SCANNING_MULT) > MAX_EL_ANGLE)
+      {
+        error(1);
+        break;
+      }
+      ELstepper.moveTo(ELsteps);
+    }
+    
+    AZstepper.run();
+    ELstepper.run();
+  }
+  /*Delay to Deccelerate*/
+  long time = millis();  
+  while(millis() - time < HOME_DELAY)
+  {   
+    AZstepper.run();
+    ELstepper.run();
+  }
+  /*Reset the steps*/
+  AZstepper.setCurrentPosition(0);
+  ELstepper.setCurrentPosition(0);   
+}
+  
 /*EasyComm 2 Protocol & Calculate the steps*/
 void cmd_proc(int &stepAz, int &stepEl)
 {
@@ -118,6 +199,34 @@ void cmd_proc(int &stepAz, int &stepEl)
   }
 }
 
+/*Error Handling*/
+void error(int num_error)
+{
+  switch (num_error)
+  {
+    /*Azimuth error*/
+    case (0):
+      while(1)
+      {
+        Serial.println("Error[0]: Azimuth Homing");
+        delay(100);
+      }
+    /*Elevation error*/
+    case (1):
+      while(1)
+      {
+        Serial.println("Error[1]: Elevation Homing");
+        delay(100);
+      }
+    default:
+      while(1)
+      {
+        Serial.println("Error[?]: Unknown error");
+        delay(100);
+      }
+  }
+}
+
 /*Send pulses to stepper motor drivers*/
 void stepper_move(int stepAz, int stepEl)
 {
@@ -138,4 +247,4 @@ int deg2step(double deg)
 double step2deg(int Step)
 {
   return(360*Step/(SPR*RATIO));
-}
\ No newline at end of file
+}