Diff: main.c

Revision:

10:94b068b2ce1d

Parent:

9:3a0433c391cb

Child:

11:a30f30d3066e

--- a/main.c	Sat Nov 15 05:07:24 2014 +0000
+++ b/main.c	Sat Nov 15 23:03:06 2014 +0000
@@ -8,6 +8,7 @@
  * @author  John Wilkey
  */
 #include "main.h"
+#include "control.h"
 
 /** 
  * These are global data Used externally in all other files 
@@ -16,7 +17,7 @@
 
 //
 // Digital inputs to the mBed
-DigitalIn   btn_b(p21);
+DigitalIn   start_button(p21);
 
 //
 // Digital outputs from the mBed. Note that by default these are
@@ -43,23 +44,27 @@
 {
     //
     // Basic setup information
-    DigitalOut olen_1(LED1);
+    start_button.mode(PullUp);
+    
     pi.locate(0,0);
     pi.printf("PiCO");
     pi.locate(0,1);
     pi.printf("%f mV", pi.battery());
+    wait(.2);
+    while(!start_button) {
+        oled_2 = 1;
+        wait(.2);
+        pi.locate(0,0);
+        pi.printf("Ready");
+        oled_2 = 0;
+    }
+    pi.locate(0,0);
+    pi.printf("GO!");
+    wait(.2);
 
     //
     // Main program loop.
-    while(1) {
-        forward(10);
-        right(90);
-        forward(10);
-        right(90);
-        forward(10);
-        wait(2);
-        left(180);
-    }
+    robot_loop();
     
     //
     // We should never reach this point!
@@ -67,36 +72,56 @@
 
 int forward(int amt)
 {
+    oled_2 = 1;
     pi.locate(0,0);
     pi.printf("Fwd %d", amt);
     pi.forward(DRIVE_SPEED);
     wait(amt*DRIVE_RATE);
+    oled_2 = 0;
     return EXIT_SUCCESS;
 }
 
 int backward(int amt)
 {
+    oled_3 = 1;
     pi.locate(0,0);
     pi.printf("Back %d", amt);
     pi.backward(DRIVE_SPEED);
     wait(amt*DRIVE_RATE);
+    oled_3 = 0;
     return EXIT_SUCCESS;
 }
 
 int right(float deg)
 {
+    oled_4 = 1;
     pi.locate(0,0);
     pi.printf("Right %f", deg);
     pi.right(TURN_SPEED);
     wait(deg/360);
+    oled_4 = 0;
     return EXIT_SUCCESS;
 }
 
 int left(float deg)
 {
+    oled_4 = 1;
+    oled_2 = 1;
     pi.locate(0,0);
     pi.printf("Left %f", deg);
     pi.left(TURN_SPEED);
     wait(deg/360);
+    oled_4 = 0;
+    oled_2 = 0;
     return EXIT_SUCCESS;
+}
+
+void pen_down()
+{
+    oled_1 = 1;
+}
+
+void pen_up()
+{
+    oled_1 = 0;
 }
\ No newline at end of file