Revision 25:2c7717684d09, committed 2014-12-08

Comitter:

alecguertin

Date:

Mon Dec 08 04:21:55 2014 +0000

Parent:

24:b797563776fc

Child:

26:a74edc4c6acb

Commit message:

pretty much working!

Changed in this revision

--- a/main.c	Sun Dec 07 09:36:59 2014 +0000
+++ b/main.c	Mon Dec 08 04:21:55 2014 +0000
@@ -23,6 +23,8 @@
 //
 // Digital outputs from the mBed. Note that by default these are
 // used to drive the 8 LED's on the top board. 
+DigitalOut  pin13(p13);
+DigitalOut  pin14(p14);
 DigitalOut  pin15(p15);
 DigitalOut  pin16(p16);
 DigitalOut  pin17(p17);
@@ -146,13 +148,8 @@
     } while(pos != -1 && pos > -0.3);
     pi.stop();
     if(pos != -1) {
-        oled_1 = 1;
         timer.stop();
-        pi.printf("T: %d", timer.read_ms());
     } else {
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("lP:%f", pos);
         return 1;
     }
     right(180);
@@ -178,25 +175,17 @@
     cal_time = caltimer.read_ms();
     pi.stop();
     if(pos != -1) {
-        oled_1 = 1;
         timer.stop();
-        pi.printf("T: %d", timer.read_ms());
     } else {
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("lP:%f", pos);
         return 1;
     }
     right(180);
     timerWait(.5);
     while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
         pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
-        // timerWait(.08);
         pi.cls();
         pi.locate(0,0);
         pi.printf("O: %f", pos);
-        // pi.stop();
-        // timerWait(.2);
     }
     pi.stop();
     timerWait(2);
@@ -214,12 +203,9 @@
     timerWait(.5);
     while(fabs(pos = pi.line_position()) > CLOSE_ENOUGH) {
         pi.right((pos < 0 ? -.6*CAL_SPEED : .6*CAL_SPEED));
-        // timerWait(.08);
         pi.cls();
         pi.locate(0,0);
         pi.printf("O: %f", pos);
-        // pi.stop();
-        // timerWait(.2);
     }
     pi.stop();
     timerWait(2);
@@ -309,7 +295,6 @@
         while(1);
     }
     */
-    // If we got here, calibration is complete.
     
     //
     // Main program loop.
@@ -330,18 +315,13 @@
     if (ps_file == NULL) {
         return 1;
     }
-    pi.cls();
-    pi.locate(0,0);
-    pi.printf("open");
+
     /* PS parsing loop. */
     memset(instbuf, 0, instbuflen);
     bytes_read = fread(instbuf, sizeof(char), instbuflen-1, ps_file);
     if (bytes_read == 0) {
         return 1;
     }
-    pi.cls();
-    pi.locate(0,0);
-    pi.printf("%.7s", instbuf);
     err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
     if (err != 2) {
         pi.cls();
@@ -356,12 +336,6 @@
     while (1) {
         memset(instbuf+offset, 0, instbuflen-offset);
         bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
-        if (bytes_read == 0) {
-            pi.cls();
-            pi.locate(0,0);
-            pi.printf("bytes0");
-            timerWait(2);
-        }
         cur = instbuf;
         err = retrieve_inst(instbuf, &x, &y, &draw);
         if (err == 0) {
@@ -374,20 +348,22 @@
         delta_y = y-last_y;
 
         /* Compute turn angle and turn. */
-        theta = atan(((double) delta_x)/((double) delta_y));
+        theta = atan(((double) delta_y)/((double) delta_x));
         theta *= 57.29;
         delta_a = theta-angle;
         if (delta_x < 0 && delta_y < 0) {
+            //pi.cls();
+            //pi.locate(0,0);
+            //pi.printf("pland");
+            //timerWait(2);
             delta_a += 180;
         }
-        angle += theta;
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("a:%f", delta_a);
-        timerWait(2);
-        if (delta_a > 0) {
-            left(delta_a);
-        }
+        angle += delta_a;
+        //pi.cls();
+        //pi.locate(0,0);
+        //pi.printf("a:%f", delta_a);
+        //timerWait(1.5);
+        left(delta_a);
 
         /* Put pen into position. */
         if (draw) {
@@ -401,10 +377,10 @@
         if (dist < 0) {
             dist *= -1;
         }
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("d:%f", 0.4*cal_time*(dist/(double)dim_x));
-        timerWait(2);
+        //pi.cls();
+        //pi.locate(0,0);
+        //pi.printf("d:%f", 0.4*cal_time*(dist/(double)dim_x));
+        //timerWait(1.5);
         forward(0.4*cal_time*(dist/(double)dim_x));
 
         last_x = x;
@@ -417,20 +393,13 @@
             break;
         }
         cur = next+1;
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("waiting!");
         offset = instbuf+instbuflen-cur-1;
         memcpy(instbuf, cur, offset);
-        pi.cls();
-        pi.locate(0,0);
-        pi.printf("%.08s", instbuf);
-        timerWait(2);
+        //pi.cls();
+        //pi.locate(0,0);
+        //pi.printf("%.08s", instbuf);
+        //timerWait(2);
     }
-    //
-    // We should never reach this point!
-    //
-    // Yes, we should...
     pi.cls();
     pi.locate(0,0);
     pi.printf("done");
@@ -468,17 +437,15 @@
 int forward(int amt)
 {
     Timer t;
-    t.start();
-    oled_2 = 1;
     pi.locate(0,0);
     pi.printf("Fwd %d", amt);
     pi.left_motor(DRIVE_SPEED+.0023);
     pi.right_motor(DRIVE_SPEED);
-    while(t.read_ms() < (amt-500));
+    t.start();
+    while(t.read_ms() < amt);
     pi.left_motor(.7*DRIVE_SPEED+.0023);
     pi.right_motor(.7*DRIVE_SPEED);
     t.stop();
-    oled_2 = 0;
     pi.stop();
     return EXIT_SUCCESS;
 }
@@ -486,16 +453,12 @@
 int backward(int amt)
 {
     Timer t;
-    oled_3 = 1;
-    pi.locate(0,0);
-    pi.printf("Back %d", amt);
     t.start();
     pi.backward(.5*DRIVE_SPEED);
     while(t.read_ms() < (amt-500));
     pi.backward(.5*DRIVE_SPEED);
     while(t.read_ms() < amt);
     t.stop();
-    oled_3 = 0;
     pi.stop();
     return EXIT_SUCCESS;
 }
@@ -506,14 +469,10 @@
         return left(-1*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() < ((float)(deg/360)*TIME_FACT));
     t.stop();
-    oled_4 = 0;
     pi.stop();
     return EXIT_SUCCESS;
 }
@@ -532,16 +491,10 @@
         return right(-1*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() < ((float)(deg/360)*TIME_FACT));
     t.stop();
-    oled_4 = 0;
-    oled_2 = 0;
     pi.stop();
     return EXIT_SUCCESS;
 }