diff --git a/c/coreslam.c b/c/coreslam.c
index 5727745..ebfecca 100644
--- a/c/coreslam.c
+++ b/c/coreslam.c
@@ -119,10 +119,7 @@ static void interpolate_scan(scan_t * scan, float * lidar_angles_deg, int * lida
     for (k=0; k<scan->size; ++k) 
     {
         lidar_distances_mm[k] = (int)interpolate(interp->angles, interp->distances, scan_size, (float)k);
-        printf("%3d: %d\n", k, lidar_distances_mm[k]);
     }
-
-    printf("\n");
 }
 
 /* Local helpers--------------------------------------------------- */
@@ -540,7 +537,6 @@ scan_update(
     {
         interpolate_scan(scan, lidar_angles_deg, lidar_distances_mm, scan_size);
     }
-    
 
     /* Take velocity into account */
     int degrees_per_second = (int)(scan->rate_hz * 360);
diff --git a/examples/rpslam.py b/examples/rpslam.py
index 3db09ed..a86999a 100755
--- a/examples/rpslam.py
+++ b/examples/rpslam.py
@@ -52,41 +52,33 @@ if __name__ == '__main__':
 
     while True:
 
-        try:
+        # Extract (quality, angle, distance) triples from current scan
+        items = [item for item in next(iterator)]
 
-            # Extract (quality, angle, distance) triples from current scan
-            items = [item for item in next(iterator)]
+        # Extract distances and angles from triples
+        distances = [item[2] for item in items]
+        angles    = [item[1] for item in items]
 
-            # Extract distances and angles from triples
-            distances = [item[2] for item in items]
-            angles    = [item[1] for item in items]
-
-            print(len(distances), len(angles))
-
-            # Update SLAM with current Lidar scan and scan angles
-            slam.update(distances, scan_angles_degrees=angles)
-
-        except KeyboardInterrupt:
-
-            break
+        # Update SLAM with current Lidar scan and scan angles
+        slam.update(distances, scan_angles_degrees=angles)
 
         # Get current robot position
-        #x, y, theta = slam.getpos()
+        x, y, theta = slam.getpos()
 
         # Get current map bytes as grayscale
-        #slam.getmap(mapbytes)
+        slam.getmap(mapbytes)
 
         # Display the map
-        #display.displayMap(mapbytes)
+        display.displayMap(mapbytes)
 
         # Display the robot's pose in the map
-        #display.setPose(x, y, theta)
+        display.setPose(x, y, theta)
 
         # Break on window close
-        #if not display.refresh():
-        #    break
+        if not display.refresh():
+            break
 
-    # Shut down the lidar connection
+# Shut down the lidar connection
     from time import sleep
     lidar.stop()
     lidar.disconnect()