Diff: main.cpp

Revision:

13:294c06406d06

Parent:

12:b107ba24faa4

--- 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");