diff --git a/PiMotor.py b/PiMotor.py
index 49852ec..18c4805 100644
--- a/PiMotor.py
+++ b/PiMotor.py
@@ -38,6 +38,7 @@ def __init__(self, motor, config):
         GPIO.output(self.pins['e'],GPIO.HIGH)
         GPIO.output(self.pins['f'],GPIO.LOW)
         GPIO.output(self.pins['r'],GPIO.LOW)
+        self.currentSpeed = 0
 
     def test(self, state):
         ''' Puts the motor into test mode
@@ -57,6 +58,9 @@ def forward(self, speed):
         0 - stop and 100 - maximum speed
         '''    
         print("Forward")
+        
+        self.currentSpeed = speed
+        
         if self.testMode:
             self.arrow.on()
         else:
@@ -72,6 +76,9 @@ def reverse(self,speed):
         0 - stop and 100 - maximum speed
      '''
         print("Reverse")
+        
+        self.currentSpeed = speed * (-1)
+        
         if self.testMode:
             self.arrow.off()
         else:
@@ -87,6 +94,12 @@ def stop(self):
         self.PWM.ChangeDutyCycle(0)
         GPIO.output(self.pins['f'],GPIO.LOW)
         GPIO.output(self.pins['r'],GPIO.LOW)
+        
+    def getSpeed(self):
+        ''' Returns current speed of the motor
+            positive numbers are forward, negative numbers are reverse '''
+            
+        return self.currentSpeed
 
     def speed(self):
         ''' Control Speed of Motor,
@@ -104,6 +117,7 @@ class LinkedMotors:
      '''
     def __init__(self, *motors):
         self.motor = []
+        self.currentSpeed = [0, 0, 0, 0]
         for i in motors:
             print(i.pins)
             self.motor.append(i)
@@ -115,7 +129,9 @@ def forward(self,speed):
         speed = Duty Cycle Percentage from 0 to 100.
         0 - stop and 100 - maximum speed 
      '''
+     
         for i in range(len(self.motor)):
+            self.currentSpeed[i] = speed
             self.motor[i].forward(speed)
 
     def reverse(self,speed):
@@ -125,8 +141,16 @@ def reverse(self,speed):
         speed = Duty Cycle Percentage from 0 to 100.
         0 - stop and 100 - maximum speed
      '''
+        
         for i in range(len(self.motor)):
+            self.currentSpeed[i] = speed
             self.motor[i].reverse(speed)
+    
+    def getSpeed(self):
+        ''' Returns array with current speed of the motors
+            positive numbers are forward, negative numbers are reverse '''
+            
+        return self.currentSpeed
 
     def stop(self):
         ''' Stops power to the motor,
@@ -237,7 +261,7 @@ def iRCheck(self):
             self.Triggered = False
 
     def sonicCheck(self):
-        print("SonicCheck has been triggered")
+        # print("SonicCheck has been triggered")
         time.sleep(0.333)
         GPIO.output(self.config["trigger"], True)
         time.sleep(0.00001)
@@ -253,7 +277,7 @@ def sonicCheck(self):
         if self.boundary > measure:
             print("Boundary breached")
             print(self.boundary)
-            print(measure)
+            print(measure / 2.54)
             self.Triggered = True
         else:
             self.Triggered = False
@@ -267,7 +291,7 @@ def trigger(self):
         If the specified "boundary" has been breached the Sensor's Triggered attribute gets set to True.
     ''' 
         self.config["check"](self)
-        print("Trigger Called")
+        # print("Trigger Called")
 
     def __init__(self, sensortype, boundary):
         self.config = self.sensorpins[sensortype]
@@ -293,9 +317,15 @@ def __init__(self, which):
         self.pin = self.arrowpins[which]
         GPIO.setup(self.pin,GPIO.OUT)
         GPIO.output(self.pin, GPIO.LOW)
+        self.isOn = False
 
     def on(self):
         GPIO.output(self.pin,GPIO.HIGH)
+        self.isOn = True
 
     def off(self):
         GPIO.output(self.pin,GPIO.LOW)
+        self.isOn = False
+        
+    def getStatus(self):
+        return self.isOn
diff --git a/Stepper_Test.py b/Stepper_Test.py
index 78ae3b4..1b7fc60 100644
--- a/Stepper_Test.py
+++ b/Stepper_Test.py
@@ -1,11 +1,18 @@
+import RPi.GPIO as GPIO                        #Import GPIO library
 import PiMotor
 import time
 
 m1 = PiMotor.Stepper("STEPPER1")
 
 # Rotate Stepper 1 Contiously in forward/backward direction
-while True:
-    m1.forward(0.1,10)  # Delay and rotations
-    time.sleep(2)
-    m1.backward(0.1,10)
-    time.sleep(2)
+
+try:
+    while True:
+        m1.forward(0.1,10)  # Delay and rotations
+        time.sleep(2)
+        m1.backward(0.1,10)
+        time.sleep(2)
+
+
+except KeyboardInterrupt:
+    GPIO.cleanup()
\ No newline at end of file
diff --git a/Test_Motor.py b/Test_Motor.py
index d5f2c03..66c3475 100644
--- a/Test_Motor.py
+++ b/Test_Motor.py
@@ -1,5 +1,6 @@
 #!/usr/bin/python
 
+import RPi.GPIO as GPIO                        #Import GPIO library
 import PiMotor
 import time