Diff: main.c

Revision:

40:0199bad6c979

Parent:

39:cc8691700d2a

Child:

41:1b6d8664d7a7

--- a/main.c	Sat Dec 13 07:35:57 2014 +0000
+++ b/main.c	Wed Dec 17 05:13:37 2014 +0000
@@ -106,7 +106,7 @@
         bytes_read = fread(instbuf+offset, sizeof(char), INST_BUF_SIZE-1-offset, ps_file);
         if (instbuf[0] == '\0') {
             robot_printf(0, "%s", instbuf+1);
-            oled_3 = 0;
+            pen_up();
             while(1);
         }
         
@@ -149,20 +149,21 @@
         robot_printf(0, "a:%f", delta_a);
         left(delta_a);
 
+        dist = fabs(distance(last_x, last_y, x, y));
+        robot_printf(0, "d:%f", CAL_FACTOR*cal_time*(dist/(double)dim_x));
+
         /* Put pen into position. */
         if (draw) {
-            oled_3 = 1;
+            pen_down();
         } else {
-            oled_3 = 0;
+            pen_up();
         }
 
         /* Compute drive time and move forward. */
-        dist = fabs(distance(last_x, last_y, x, y));
-        robot_printf(0, "d:%f", CAL_FACTOR*cal_time*(dist/(double)dim_x));
         forward(CAL_FACTOR*cal_time*(dist/(double)dim_x));
+        pen_up();
         last_x = x;
         last_y = y;
-        oled_3 = 0;
         
         /* Seek to find next instruction. */
         next = strchr(cur, '\n');
@@ -176,7 +177,7 @@
     }
     robot_printf(0, "Done");
     pi.stop(); 
-    oled_3 = 0;
+    pen_up();
     while (1);
 }
 
@@ -369,4 +370,14 @@
     Timer t;
     t.start();
     while(t.read_us() < 1000000*seconds);
-}
\ No newline at end of file
+}
+
+void pen_down()
+{
+    oled_3 = 1;
+}
+
+void pen_up()
+{
+    oled_3 = 0;
+}