diff --git a/Sources/SwiftRPLidar/SwiftRPLidar.swift b/Sources/SwiftRPLidar/SwiftRPLidar.swift
index 9c8ac86..302c7ed 100644
--- a/Sources/SwiftRPLidar/SwiftRPLidar.swift
+++ b/Sources/SwiftRPLidar/SwiftRPLidar.swift
@@ -40,10 +40,10 @@ func processScan(raw: Data) throws -> LidarScan {
     let inversedNewScan = (raw[0] >> 1) & 0b1
     let quality = raw[0] >> 2
     if (newScan == inversedNewScan){
-
+        
     }
     if ((raw[1] & 0b1) != 1) {
-
+        
     }
     let angle = Float(raw[1] >> 1) + Float(raw[2] << 7) / 64
     let distance = Float(raw[3]) + Float(raw[4] << 8) / 4
@@ -59,8 +59,8 @@ struct LidarScan{
 
 
 
-typealias MeasurementHandler = (_ scan: LidarScan) -> Void
-typealias ScanHandler = (_ scans: [LidarScan]) -> Void
+typealias MeasurementHandler = (_ scan: LidarScan) -> Bool
+typealias ScanHandler = (_ scans: [LidarScan]) -> Bool
 
 class RPLidar{
     private var motor: Bool = false
@@ -68,9 +68,10 @@ class RPLidar{
     private var motorRunning = false
     
     
-    init(serialPort: SerialPort) throws {
+    init(onPort serialPort: SerialPort) throws {
         self.serialPort = serialPort
-
+        try connect()
+        try startMotor()
     }
     
     deinit {
@@ -164,7 +165,7 @@ class RPLidar{
         if status == .ERROR{
             // Throw Exception
         }
-        
+            
         else if status == .WARNING {
             
         }
@@ -180,9 +181,9 @@ class RPLidar{
         if dataType != Constants.SCAN_TYPE {
             
         }
-        
+        var read = true
         // Need to check in waiting or something...
-        for _ in 0..<500{
+        while read {
             let raw = try readResponse(Int(dataSize))
             if maxBufferMeasurements > 0 {
                 let dataInWaiting = serialPort?.inWaiting
@@ -192,22 +193,24 @@ class RPLidar{
                 }
             }
             // TODO: Support cancelling of measurements. Would it already work though?
-            try onMeasure(processScan(raw: raw))
+            read = try onMeasure(processScan(raw: raw))
         }
     }
     
     public func iterScans(maxBufferMeasurements: Int = 500, minLength: Int = 5, _ onScan: ScanHandler) throws {
         var scan: [LidarScan] = []
+        var read = true
         try iterMeasurements{ measurement in
             if measurement.newScan {
                 if scan.count > minLength {
-                    onScan(scan)
+                    read = onScan(scan)
                 }
                 scan = []
             }
             if measurement.quality > 0 && measurement.distance > 0 {
                 scan.append(measurement)
             }
+            return read
         }
         
     }
@@ -243,10 +246,10 @@ class RPLidar{
             return nil
         }
         if (descriptor.count != Constants.DESCRIPTOR_LEN){
-            return
+            return nil
         }
         else if (descriptor[0...1] != Data([Constants.SYNC, Constants.SYNC2])){
-            return
+            return nil
         }
         let isSingle = descriptor[descriptor.count - 2] == 0
         return (descriptor[2], isSingle, descriptor[descriptor.count - 1])
@@ -257,7 +260,7 @@ class RPLidar{
             return Data()
         }
         if(data.count != dataSize){
-            return
+            return Data()
         }
         return data
     }