Revision 13:294c06406d06, committed 2018-05-02

Comitter:

jsterling30

Date:

Wed May 02 14:25:43 2018 +0000

Parent:

12:b107ba24faa4

Commit message:

demo changes

Changed in this revision

--- a/letters.h	Tue May 01 16:17:24 2018 +0000
+++ b/letters.h	Wed May 02 14:25:43 2018 +0000
@@ -1,11 +1,83 @@
+/*
 typedef struct {
     double x;
     double y;
-    bool pen;
+    int pen;
 } Transition;
 
 typedef struct {
-    Transition points[7];
+    Transition points[10];
 } Letter;
 
-Letter let_a = { .points = { {0.0, 0.0, false}, {0.25, 0.25, true}, {1.0, 1.175, true}, {1.75, .25, true}, {.625, 1.0, false}, {1.375, 1.0, true}, {2.0, 0.0, false} } };
\ No newline at end of file
+Letter let_a = { .points = { {0.0, 0.0, 0}, {0.25, 0.25, 1}, {1.0, 1.175, 1}, {1.75, .25, 1}, {.625, 1.0, 0}, {1.375, 1.0, 1}, {2.0, 0.0, 0} } };
+Letter let_h = { .points = { {0.0, 0.0, 0}, {0.0, 2.0, 1}, {2.0, 2.0, 0}, {2.0, 0.0, 1}, {0.0, 1.0, 0}, {2.0, 1.0, 1}, {2.0, 0.0, 0} } };
+Letter let_l = { .points = { {0.0, 0.0, 0}, {0.25, 0.25, 1}, {0.25, 1.75, 1}, {0.25, 0.25, 0}, {1.25, 0.25, 1}, {2.0, 0.0, 0}}};
+Letter let_e = { .points = { {0.0, 0.0, 0}, {0.25, 0.25, 1}, {0.25, 1.75, 1}, {1, 1.75, 1}, {0.25, 1, 0}, {1, 1, 1}, {0.25, 0.25, 0}, {1, 0.25, 1}, {2.0, 0.0, 0}}};
+Letter let_o = { .points = { {0.0, 0.0, 0}, {0.25, 0.25, 1}, {0.25, 1.75, 1}, {1, 1.75, 1}, {1, 0.25, 1}, {0.25, 0.25, 1}, {2.0, 0.0, 0}}};
+*/
+
+#include <math.h>
+#define PI 3.1415926535
+
+class Transition {
+    public:
+        double x_init;
+        double x_final;
+        double y_init;
+        double y_final;
+        int draw;
+        
+        Transition (double x_i, double y_i, double x_f, double y_f, int d) {
+            x_init = x_i;
+            x_final = x_f;
+            y_init = y_i;
+            y_final = y_f;
+            draw = d;
+        }
+        
+        Transition () {}
+        
+        double desired_angle() {
+            return atan2(y_final - y_init, x_final - x_init) * 180.0 / PI;
+        }
+        
+        double distance() {
+            return sqrt(pow(y_final - y_init, 2) + pow(x_final - x_init, 2));
+        }
+};
+
+Transition h1(0,0,.25,.25,false);
+Transition h2(.25,.25,.25,1.75,true);
+Transition h3(.25,1.75,.25,1,false);
+Transition h4(.25,1,1,1,true);
+Transition h5(1,1,1,1.75,false);
+Transition h6(1,1.75,1,.25,true);
+Transition h7(1,.25,2,0,false);
+
+Transition let_h[7] = { h1, h2, h3, h4, h5, h6, h7 };
+
+Transition e1(0,0,1,1.75,false);
+Transition e2(1,1.75,0.25,1.75,true);
+Transition e3(0.25,1.75,.25,.25,true);
+Transition e4(.25,.25,1,.25,true);
+Transition e5(1,.25,.25,1,false);
+Transition e6(.25,1,1,1,true);
+Transition e7(1,1,2,0,false);
+
+Transition let_e[7] = { e1, e2, e3, e4, e5, e6, e7 };
+
+Transition l1(0,0,0.25,1.75,false);
+Transition l2(0.25,1.75,0.25,0.25,true);
+Transition l3(0.25,0.25,1.25,0.25,true);
+Transition l4(1.25,0.25,2,0,false);
+
+Transition let_l[4] = { l1, l2, l3, l4 };
+
+Transition o1(0,0,.25,.25,false);
+Transition o2(.25,.25,.25,1.75,true);
+Transition o3(.25,1.75,1.75,1.75,true);
+Transition o4(1.75,1.75,1.75,.25,true);
+Transition o5(1.75,.25,.25,.25,true);
+Transition o6(.25,.25,2,0,false);
+
+Transition let_o[6] = { o1, o2, o3, o4, o5, o6 };
\ No newline at end of file

--- a/main.cpp	Tue May 01 16:17:24 2018 +0000
+++ b/main.cpp	Wed May 02 14:25:43 2018 +0000
@@ -10,9 +10,9 @@
 #define SPEED 0.33
 #define TICKSPERREV 390
 #define DISTPERREV 8.25         // 8.25 inches per revolution
-#define TicksPerDeg 2.73
+#define TicksPerDeg 2.42
 
-#define PI 3.14159265
+#define PI 3.1415926535
 #define abs(X) ((X < 0) ? -1 * X : X)
  
 Motor right(p23, p6, p5, 1);    // pwm, fwd, rev
@@ -51,9 +51,38 @@
     double numTicks = numRevs * TICKSPERREV;
     leftEnc.reset();
     rightEnc.reset();
-    right.speed(SPEED);
-    left.speed(SPEED);
-    while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
+    double leftSpeed = SPEED;
+    double rightSpeed = SPEED;
+    
+    int leftTicksPrev = 0;
+    int rightTicksPrev = 0;
+    double offset = 0.01;
+    
+    while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {
+        
+        int leftTicks = leftEnc.read();
+        int rightTicks = rightEnc.read();
+        
+        int leftDiff = leftTicks - leftTicksPrev;
+        int rightDiff = rightTicks - rightTicksPrev;
+        
+        leftTicksPrev = leftTicks;
+        rightTicksPrev = rightTicks;
+        
+        if (leftDiff > rightDiff) {
+            leftSpeed -= offset;
+            rightSpeed += offset;
+        } else if (leftDiff < rightDiff) {
+            leftSpeed += offset;
+            rightSpeed -= offset;
+        }
+        
+        right.speed(rightSpeed);
+        left.speed(leftSpeed);
+        
+        wait(0.1);
+        
+    }
     stop();
     wait(0.5);
 }
@@ -61,11 +90,42 @@
 void reverse(double distance) {
     double numRevs = distance / DISTPERREV;
     double numTicks = numRevs * TICKSPERREV;
+    
     leftEnc.reset();
     rightEnc.reset();
-    right.speed(-SPEED);
-    left.speed(-SPEED);
-    while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
+    
+    double leftSpeed = -SPEED;
+    double rightSpeed = -SPEED;
+    
+    int leftTicksPrev = 0;
+    int rightTicksPrev = 0;
+    double offset = 0.01;
+    
+    while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {
+        
+        int leftTicks = leftEnc.read();
+        int rightTicks = rightEnc.read();
+        
+        int leftDiff = leftTicks - leftTicksPrev;
+        int rightDiff = rightTicks - rightTicksPrev;
+        
+        leftTicksPrev = leftTicks;
+        rightTicksPrev = rightTicks;
+        
+        if (leftDiff > rightDiff) {
+            leftSpeed += offset;
+            rightSpeed -= offset;
+        } else if (leftDiff < rightDiff) {
+            leftSpeed -= offset;
+            rightSpeed += offset;
+        }
+        
+        right.speed(rightSpeed);
+        left.speed(leftSpeed);
+        
+        wait(0.1);
+        
+    }
     stop();
     wait(0.5);
 }
@@ -73,16 +133,40 @@
 void turnLeft(double degrees) {
     leftEnc.reset();
     rightEnc.reset();
-    right.speed(SPEED + 0.05);
-    left.speed(-SPEED - 0.05);
+    
+    double rightSpeed = SPEED + .04;
+    double leftSpeed = -SPEED - .04;
+    
+    int leftTicksPrev = 0;
+    int rightTicksPrev = 0;
+    double offset = 0.01;
+    
     double numTicks = degrees * TicksPerDeg;
+    
     while (leftEnc.read() < numTicks || rightEnc.read() < numTicks) {
-        if (leftEnc.read() >= numTicks) {
-            left.speed(0.0);
+        
+        int leftTicks = leftEnc.read();
+        int rightTicks = rightEnc.read();
+        
+        int leftDiff = leftTicks - leftTicksPrev;
+        int rightDiff = rightTicks - rightTicksPrev;
+        
+        leftTicksPrev = leftTicks;
+        rightTicksPrev = rightTicks;
+        
+        if (leftDiff > rightDiff) {
+            leftSpeed += offset;
+            rightSpeed += offset;
+        } else if (leftDiff < rightDiff) {
+            leftSpeed -= offset;
+            rightSpeed -= offset;
         }
-        if (rightEnc.read() >= numTicks) {
-            right.speed(0.0);
-        }
+        
+        right.speed(rightSpeed);
+        left.speed(leftSpeed);
+        
+        wait(0.1);
+        
     }
     stop();
     wait(0.5);
@@ -91,31 +175,61 @@
 void turnRight(double degrees) {
     leftEnc.reset();
     rightEnc.reset();
-    right.speed(-SPEED - 0.05);
-    left.speed(SPEED + 0.05);
+    
+    double rightSpeed = -SPEED - .04;
+    double leftSpeed = SPEED + .04;
+    
+    int leftTicksPrev = 0;
+    int rightTicksPrev = 0;
+    double offset = 0.01;
+    
     double numTicks = degrees * TicksPerDeg;
-    while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
+    
+    while (leftEnc.read() < numTicks || rightEnc.read() < numTicks) {
+        
+        int leftTicks = leftEnc.read();
+        int rightTicks = rightEnc.read();
+        
+        int leftDiff = leftTicks - leftTicksPrev;
+        int rightDiff = rightTicks - rightTicksPrev;
+        
+        leftTicksPrev = leftTicks;
+        rightTicksPrev = rightTicks;
+        
+        if (leftDiff > rightDiff) {
+            leftSpeed -= offset;
+            rightSpeed -= offset;
+        } else if (leftDiff < rightDiff) {
+            leftSpeed += offset;
+            rightSpeed += offset;
+        }
+        
+        right.speed(rightSpeed);
+        left.speed(leftSpeed);
+        
+        wait(0.1);
+        
+    }
     stop();
     wait(0.5);
 }
 
 void turn(double delta) {
     penUp();
-    forward(1.75);
+    forward(1.5);
     if (delta > 0) {
         turnLeft(delta);
     } else {
         turnRight(-delta);
     }
-    reverse(1.75);
-    penDown();
+    reverse(1.5);
 }
 
 void makeCircle() {
     penDown();
     leftEnc.reset();
     rightEnc.reset();
-    right.speed(0.4);
+    right.speed(0.5);
     float numTicks = 2 * 360 * TicksPerDeg;
     while (rightEnc.read() < numTicks) {}
     stop();
@@ -126,10 +240,13 @@
     penDown();
     forward(sideLength);
     turn(90);
+    penDown();
     forward(sideLength);
     turn(90);
+    penDown();
     forward(sideLength);
     turn(90);
+    penDown();
     forward(sideLength);
     turn(90);
 }
@@ -138,51 +255,58 @@
     penDown();
     forward(sideLength);
     turn(120);
+    penDown();
     forward(sideLength);
     turn(120);
+    penDown();
     forward(sideLength);
-    turn(120);
 }
 
 void makeHexagon(int sideLength) {
     penDown();
     forward(sideLength);
     turn(60);
+    penDown();
     forward(sideLength);
     turn(60);
+    penDown();
     forward(sideLength);
     turn(60);
+    penDown();
     forward(sideLength);
     turn(60);
+    penDown();
     forward(sideLength);
     turn(60);
+    penDown();
     forward(sideLength);
     turn(60);
 }
 
-void makePattern(string shape, int sideLength){
-    if (shape.compare("triangle") == 0) {
+void makePattern(char *shape, int sideLength){
+    if (strcmp(shape, "triangle") == 0) {
+        makeTriangle(sideLength);
+        //turn(150);
         makeTriangle(sideLength);
-        turn(120);
+        //turn(150);
         makeTriangle(sideLength);
-        turn(120);
-        makeTriangle(sideLength);
+        //turn(150);
+        //makeTriangle(sideLength);
+    } else if (strcmp(shape, "circle") == 0) {
+        makeCircle();
         turn(120);
-    } else if (shape.compare("circle") == 0) {
-        makeCircle(sideLength);
+        makeCircle();
         turn(120);
-        makeCircle(sideLength);
+        makeCircle();
         turn(120);
-        makeCircle(sideLength);
-        turn(120);
-    } else if (shape.compare("hexagon") == 0) {
+    } else if (strcmp(shape, "hexagon") == 0) {
         makeHexagon(sideLength);
         turn(120);
         makeHexagon(sideLength);
         turn(120);
         makeHexagon(sideLength);
         turn(120);
-    } else if (shape.compare("square") == 0) {
+    } else if (strcmp(shape, "square") == 0) {
         makeSquare(sideLength);
         turn(120);
         makeSquare(sideLength);
@@ -219,21 +343,67 @@
     }
 }
 
-void drawLetter(Letter letter) {
-    double currAngle = 0;
-    for (int i = 1; i < sizeof(letter.points); i++) {
-        Transition prev = letter.points[i - 1];
-        Transition next = letter.points[i];
-        double desAngle = atan2(next.y - prev.y, next.x - prev.x) * 180 / PI;
-        double theta = desAngle - currAngle;
-        if (theta > 0) {
-            turnLeft(theta);
+Transition firstTransition(0,0,.25,.25,false);
+
+void drawLetter(Transition letter[], int numSteps) {
+    double currAngle = 0.0;
+    for (int i = 0; i < numSteps; i++) {
+        double desAngle = letter[i].desired_angle();
+        double delta = desAngle - currAngle;
+        double distance = letter[i].distance();
+        
+        if (delta > 90.0) {
+            delta -= 180;
+            turn(delta);
+            if (letter[i].draw == true) {
+                penDown();
+            } else {
+                penUp();
+            }
+            reverse(distance);
+        } else if (delta < -90.0) {
+            delta += 180;
+            turn(delta);
+            if (letter[i].draw == true) {
+                penDown();
+            } else {
+                penUp();
+            }
+            reverse(distance);
         } else {
-            turnRight(-theta);
+            turn(delta);
+            if (letter[i].draw == true) {
+                penDown();
+            } else {
+                penUp();
+            }
+            forward(distance);
         }
-        currAngle = desAngle;
-        double distance = sqrt(pow(next.y - prev.y, 2) + pow(next.x - prev.x, 2));
-        forward(distance);
+        
+        currAngle += delta;
+    }
+    double delta = 0 - currAngle;
+    turn(delta);
+}
+
+void drawWord(char *word) {
+    for (int i = 0; i < strlen(word); i++) {
+        switch (word[i]) {
+            case 'h':
+                drawLetter(let_h, 7);
+                break;
+            case 'e':
+                drawLetter(let_e, 7);
+                break;
+            case 'l':
+                drawLetter(let_l, 4);
+                break;
+            case 'o':
+                drawLetter(let_o, 6);
+                break;
+            default:
+                blue.printf("    Letter not recognized\n");
+        }
     }
 }
 
@@ -254,8 +424,8 @@
         char *arg3 = strtok(NULL, delimiter);
         
         if (strcmp(arg1, "draw") == 0) {
+            
             // first argument is draw
-            
             if (strcmp(arg2, "square") == 0) {
                 // second argument is square
                 int sideDistance = 3;
@@ -290,9 +460,25 @@
             }
             
         } else if (strcmp(arg1, "write") == 0) {
+            
             // first argument is write
-            blue.printf("First argument is write\n");
-            blue.printf("Second argument is %s\n", arg2);
+            blue.printf("Writing the word: %s\n", arg2);
+            drawWord(arg2);
+            
+        } else if (strcmp(arg1, "pattern") == 0) {
+            
+            // first argument is pattern
+            if ((strcmp(arg2, "square") != 0) && (strcmp(arg2, "circle") != 0) && (strcmp(arg2, "triangle") != 0) && (strcmp(arg2, "hexagon") != 0)) {
+                blue.printf("ERROR: Second argument is not recognized (square, circle, triangle, hexagon)\n");
+            } else {
+                int sideDistance = 3;
+                if (arg3 != NULL) {
+                    sideDistance = arg3[0] - '0';
+                }
+                blue.printf("Drawing %s pattern with side length = %d\n", arg2, sideDistance);
+                makePattern(arg2, sideDistance);
+            }
+            
         } else {
             // first argument is not recognized
             blue.printf("ERROR: First argument is not recognized (must be draw or write)\n");