Revision 19:47759cf4f9b9, committed 2014-12-01

Comitter:

alecguertin

Date:

Mon Dec 01 20:16:23 2014 +0000

Parent:

18:eab7b0e89398

Child:

20:76718145b403

Commit message:

added logic for turning/driving

Changed in this revision

--- a/main.c	Mon Dec 01 01:02:42 2014 +0000
+++ b/main.c	Mon Dec 01 20:16:23 2014 +0000
@@ -5,7 +5,7 @@
  * Maybe add lots of stuff here or maybe split it off into
  * multiple subfiles?
  *
- * @author  John Wilkey
+ * @author  John Wilkey - aw hells no! you ain't takin' credit for all this!
  */
 #include "main.h"
 #include "control.h"
@@ -63,7 +63,8 @@
     float   pos         = 0;
     float   over_thresh = 0.05;
     float   correction  = 0.2*DRIVE_SPEED;
-
+    float   cal_time;
+    
     wait(1);
 
     do {
@@ -148,6 +149,7 @@
     if(pos != -1) {
         oled_1 = 1;
         timer.stop();
+        cal_time = timer.read();
         pi.printf("T: %d", timer.read_ms());
     } else {
         pi.cls();
@@ -175,7 +177,7 @@
     if(pos == 1) {
         oled_1 = 1;
         timer.stop();
-        pi.stop();           
+        pi.stop();
     } else {
         pi.stop();
         pi.cls();
@@ -189,14 +191,25 @@
     // Main program loop.
     // robot_loop();
     size_t bytes_read = 0;
-    int err, x, y, last_x, last_y, delta_x, delta_y;
+    int err, x, y, last_x, last_y, delta_x, delta_y, delta_a;
+    int dim_x, dim_y;
     int offset = 0;
     char *cur, *next;
-    float angle, theta;
+    float angle;
+    double dist, theta;
     angle = 0;
     theta = 0;
+    last_x = 0;
+    last_y = 0;
     ps_file = fopen("/local/test.ps", "r");
     /* PS parsing loop. */
+    memset(instbuf, 0, instbuflen);
+    bytes_read = fread(instbuf, sizeof(char), instbuflen, ps_file);
+    err = sscanf(instbuf, "%d/%d", &dim_x, &dim_y);
+    if (err != 2) {
+        return 1;
+    }
+    cur = strchr(instbuf, '\n');
     while (1) {
         memset(instbuf+offset, 0, instbuflen-offset);
         bytes_read = fread(instbuf+offset, sizeof(char), instbuflen-1-offset, ps_file);
@@ -206,11 +219,37 @@
         cur = instbuf;
         while (cur[0] != '\0') {
             err = retrieve_inst(instbuf, &x, &y, &draw);
+            if (err == 0) {
+                return 1;
+            }
             delta_x = x-last_x;
             delta_y = y-last_y;
-            theta = tan(delta_x/delta_y);
+
+            /* Compute turn angle and turn. */
+            theta = tan((double) delta_x/delta_y);
             theta *= 57.2957795;
             delta_a = theta-angle;
+            if (delta_a > 0) {
+                pi.left(TURN_SPEED);
+            } else {
+                pi.right(TURN_SPEED);
+            }
+            wait(0.5*(delta_a/360));
+
+            /* Put pen into position. */
+            if (draw) {
+                oled_1 = 1;
+            } else {
+                oled_1 = 0;
+            }
+            /* Compute drive time and move forward. */
+            dist = sqrt(pow((double) (delta_x),2) + pow((double) (delta_y), 2));
+            if (dist < 0) {
+                dist *= -1;
+            }
+            pi.forward(DRIVE_SPEED);
+            wait(cal_time*(dist/dim_x));
+
             last_x = x;
             last_y = y;
             next = strchr(cur, '\n');