Diff: main.c.orig

Revision:

16:f1beef7beeb9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.c.orig	Sun Nov 30 21:52:58 2014 +0000
@@ -0,0 +1,192 @@
+/**
+ * @file    driver.c
+ * @brief   Basic driver program for our robot's controller logic. 
+ *
+ * Maybe add lots of stuff here or maybe split it off into
+ * multiple subfiles?
+ *
+ * @author  John Wilkey
+ */
+#include "main.h"
+#include "control.h"
+
+/** 
+ * These are global data Used externally in all other files 
+ */
+m3pi        pi;
+Timer       timer;
+
+//
+// Digital inputs to the mBed
+DigitalIn   start_button(p21);
+
+//
+// Digital outputs from the mBed. Note that by default these are
+// used to drive the 8 LED's on the top board. 
+DigitalOut  pin15(p15);
+DigitalOut  pin16(p16);
+DigitalOut  pin17(p17);
+DigitalOut  pin18(p18);
+DigitalOut  pin19(p19);
+DigitalOut  pin20(p20);
+
+// 
+// mBed onboard LEDs
+DigitalOut  oled_1(LED1);
+DigitalOut  oled_2(LED2);
+DigitalOut  oled_3(LED3);
+DigitalOut  oled_4(LED4);
+
+
+/**
+ * @brief Entry point. Main loop.
+ */
+int main()
+{
+    //
+    // Basic setup information
+    start_button.mode(PullUp);
+        
+    //
+    // Drawing environment calibration.
+    pi.sensor_auto_calibrate();
+    pi.backward(DRIVE_SPEED);
+    float   pos;
+    float   over_thresh = 0.5;
+    float   correction  = 0.1;
+    
+    do {
+        pos = pi.line_position();
+        if(pos > over_thresh) {
+            pi.right_motor(DRIVE_SPEED);
+            pi.left_motor(DRIVE_SPEED - correction);
+        } else if(pos < -over_thresh) {
+            pi.left_motor(DRIVE_SPEED);
+            pi.right_motor(DRIVE_SPEED - correction);
+        } else {
+            pi.forward(DRIVE_SPEED);
+        }
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("P: %f", pos);
+    } while(pos != -1 && pos != 1);
+    if(pos == 1) {
+        timer.start();
+        pi.forward(DRIVE_SPEED);
+    } else {
+        pi.stop();
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("LP: %f",pos);
+        while(1);
+    }
+
+    while(pi.line_position() == 1);
+    do {
+        pos = pi.line_position();
+        if(pos > over_thresh) {
+            pi.right_motor(DRIVE_SPEED);
+            pi.left_motor(DRIVE_SPEED - correction);
+        } else if(pos < -over_thresh) {
+            pi.left_motor(DRIVE_SPEED);
+            pi.right_motor(DRIVE_SPEED - correction);
+        } else {
+            pi.forward(DRIVE_SPEED);
+        }
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("Pos: %f", pos);
+    } while(pos != -1 && pos != 1);
+    if(pos == 1) {
+        oled_1 = 1;
+        timer.stop();
+        pi.stop();           
+    } else {
+        pi.stop();
+        pi.cls();
+        pi.locate(0,0);
+        pi.printf("LP:%f", pos);
+        while(1);
+    }
+    // If we got here, calibration is complete.
+    
+    //
+    // Main program loop.
+    // robot_loop();
+    
+    //
+    // We should never reach this point!
+    while(1);
+}
+
+int forward(int amt)
+{
+    Timer t;
+    t.start();
+    oled_2 = 1;
+    pi.locate(0,0);
+    pi.printf("Fwd %d", amt);
+    pi.forward(DRIVE_SPEED);
+    while(t.read_ms() < amt*DRIVE_RATE*1000);
+    t.stop();
+    oled_2 = 0;
+    pi.stop();
+    return EXIT_SUCCESS;
+}
+
+int backward(int amt)
+{
+    Timer t;
+    oled_3 = 1;
+    pi.locate(0,0);
+    pi.printf("Back %d", amt);
+    pi.backward(DRIVE_SPEED);
+    t.start();
+    while(t.read_ms() < amt*DRIVE_RATE*1000);
+    t.stop();
+    oled_3 = 0;
+    pi.stop();
+    return EXIT_SUCCESS;
+}
+
+int right(float deg)
+{
+    Timer t;
+    oled_4 = 1;
+    pi.locate(0,0);
+    pi.printf("Right %f", deg);
+    pi.right(TURN_SPEED);
+    t.start();
+    while(t.read_ms() < (deg/360)*1000);
+    t.stop();
+    oled_4 = 0;
+    pi.stop();
+    return EXIT_SUCCESS;
+}
+
+int left(float deg)
+{
+    Timer t;
+    oled_4 = 1;
+    oled_2 = 1;
+    pi.locate(0,0);
+    pi.printf("Left %f", deg);
+    pi.left(TURN_SPEED);
+    t.start();
+    while(t.read_ms() < (deg/360)*1000);
+    t.stop();
+    oled_4 = 0;
+    oled_2 = 0;
+    pi.stop();
+    return EXIT_SUCCESS;
+}
+
+void pen_down()
+{
+    oled_1 = 1;
+}
+
+void pen_up()
+{
+    oled_1 = 0;
+}
\ No newline at end of file