main.cpp

00001 #include "mbed.h"
00002 #include "motordriver.h"
00003 #include "HALLFX_ENCODER.h"
00004 #include "Servo.h"
00005 #include "letters.h"
00006 
00007 #include <math.h>
00008 #include <string.h>
00009 
00010 #define SPEED 0.33
00011 #define TICKSPERREV 390
00012 #define DISTPERREV 8.25         // 8.25 inches per revolution
00013 #define TicksPerDeg 2.42
00014 
00015 #define PI 3.1415926535
00016 #define abs(X) ((X < 0) ? -1 * X : X)
00017  
00018 Motor right(p23, p6, p5, 1);    // pwm, fwd, rev
00019 Motor left(p21, p7, p8, 1);     // pwm, fwd, rev
00020 
00021 HALLFX_ENCODER leftEnc(p15);
00022 HALLFX_ENCODER rightEnc(p16);
00023 
00024 Serial blue(p13,p14);
00025 Serial pc(USBTX, USBRX);
00026 DigitalOut led1(LED1);
00027 DigitalOut led4(LED4);
00028 
00029 Servo pen(p24);
00030 
00031 double penUpValue = 0.8;
00032 double penDownValue = 1.0;
00033 
00034 void penUp() {
00035     pen = penUpValue;
00036     wait(0.5);
00037 }
00038 
00039 void penDown() {
00040     pen = penDownValue;
00041     wait(0.5);
00042 }
00043 
00044 void stop() {
00045     right.speed(0.0);
00046     left.speed(0.0);
00047 }
00048 
00049 void forward(double distance) {
00050     double numRevs = distance / DISTPERREV;
00051     double numTicks = numRevs * TICKSPERREV;
00052     leftEnc.reset();
00053     rightEnc.reset();
00054     double leftSpeed = SPEED;
00055     double rightSpeed = SPEED;
00056     
00057     int leftTicksPrev = 0;
00058     int rightTicksPrev = 0;
00059     double offset = 0.01;
00060     
00061     while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {
00062         
00063         int leftTicks = leftEnc.read();
00064         int rightTicks = rightEnc.read();
00065         
00066         int leftDiff = leftTicks - leftTicksPrev;
00067         int rightDiff = rightTicks - rightTicksPrev;
00068         
00069         leftTicksPrev = leftTicks;
00070         rightTicksPrev = rightTicks;
00071         
00072         if (leftDiff > rightDiff) {
00073             leftSpeed -= offset;
00074             rightSpeed += offset;
00075         } else if (leftDiff < rightDiff) {
00076             leftSpeed += offset;
00077             rightSpeed -= offset;
00078         }
00079         
00080         right.speed(rightSpeed);
00081         left.speed(leftSpeed);
00082         
00083         wait(0.1);
00084         
00085     }
00086     stop();
00087     wait(0.5);
00088 }
00089 
00090 void reverse(double distance) {
00091     double numRevs = distance / DISTPERREV;
00092     double numTicks = numRevs * TICKSPERREV;
00093     
00094     leftEnc.reset();
00095     rightEnc.reset();
00096     
00097     double leftSpeed = -SPEED;
00098     double rightSpeed = -SPEED;
00099     
00100     int leftTicksPrev = 0;
00101     int rightTicksPrev = 0;
00102     double offset = 0.01;
00103     
00104     while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {
00105         
00106         int leftTicks = leftEnc.read();
00107         int rightTicks = rightEnc.read();
00108         
00109         int leftDiff = leftTicks - leftTicksPrev;
00110         int rightDiff = rightTicks - rightTicksPrev;
00111         
00112         leftTicksPrev = leftTicks;
00113         rightTicksPrev = rightTicks;
00114         
00115         if (leftDiff > rightDiff) {
00116             leftSpeed += offset;
00117             rightSpeed -= offset;
00118         } else if (leftDiff < rightDiff) {
00119             leftSpeed -= offset;
00120             rightSpeed += offset;
00121         }
00122         
00123         right.speed(rightSpeed);
00124         left.speed(leftSpeed);
00125         
00126         wait(0.1);
00127         
00128     }
00129     stop();
00130     wait(0.5);
00131 }
00132 
00133 void turnLeft(double degrees) {
00134     leftEnc.reset();
00135     rightEnc.reset();
00136     
00137     double rightSpeed = SPEED + .04;
00138     double leftSpeed = -SPEED - .04;
00139     
00140     int leftTicksPrev = 0;
00141     int rightTicksPrev = 0;
00142     double offset = 0.01;
00143     
00144     double numTicks = degrees * TicksPerDeg;
00145     
00146     while (leftEnc.read() < numTicks || rightEnc.read() < numTicks) {
00147         
00148         int leftTicks = leftEnc.read();
00149         int rightTicks = rightEnc.read();
00150         
00151         int leftDiff = leftTicks - leftTicksPrev;
00152         int rightDiff = rightTicks - rightTicksPrev;
00153         
00154         leftTicksPrev = leftTicks;
00155         rightTicksPrev = rightTicks;
00156         
00157         if (leftDiff > rightDiff) {
00158             leftSpeed += offset;
00159             rightSpeed += offset;
00160         } else if (leftDiff < rightDiff) {
00161             leftSpeed -= offset;
00162             rightSpeed -= offset;
00163         }
00164         
00165         right.speed(rightSpeed);
00166         left.speed(leftSpeed);
00167         
00168         wait(0.1);
00169         
00170     }
00171     stop();
00172     wait(0.5);
00173 }
00174 
00175 void turnRight(double degrees) {
00176     leftEnc.reset();
00177     rightEnc.reset();
00178     
00179     double rightSpeed = -SPEED - .04;
00180     double leftSpeed = SPEED + .04;
00181     
00182     int leftTicksPrev = 0;
00183     int rightTicksPrev = 0;
00184     double offset = 0.01;
00185     
00186     double numTicks = degrees * TicksPerDeg;
00187     
00188     while (leftEnc.read() < numTicks || rightEnc.read() < numTicks) {
00189         
00190         int leftTicks = leftEnc.read();
00191         int rightTicks = rightEnc.read();
00192         
00193         int leftDiff = leftTicks - leftTicksPrev;
00194         int rightDiff = rightTicks - rightTicksPrev;
00195         
00196         leftTicksPrev = leftTicks;
00197         rightTicksPrev = rightTicks;
00198         
00199         if (leftDiff > rightDiff) {
00200             leftSpeed -= offset;
00201             rightSpeed -= offset;
00202         } else if (leftDiff < rightDiff) {
00203             leftSpeed += offset;
00204             rightSpeed += offset;
00205         }
00206         
00207         right.speed(rightSpeed);
00208         left.speed(leftSpeed);
00209         
00210         wait(0.1);
00211         
00212     }
00213     stop();
00214     wait(0.5);
00215 }
00216 
00217 void turn(double delta) {
00218     penUp();
00219     forward(1.5);
00220     if (delta > 0) {
00221         turnLeft(delta);
00222     } else {
00223         turnRight(-delta);
00224     }
00225     reverse(1.5);
00226 }
00227 
00228 void makeCircle() {
00229     penDown();
00230     leftEnc.reset();
00231     rightEnc.reset();
00232     right.speed(0.5);
00233     float numTicks = 2 * 360 * TicksPerDeg;
00234     while (rightEnc.read() < numTicks) {}
00235     stop();
00236     wait(0.1);
00237 }
00238 
00239 void makeSquare(int sideLength) {
00240     penDown();
00241     forward(sideLength);
00242     turn(90);
00243     penDown();
00244     forward(sideLength);
00245     turn(90);
00246     penDown();
00247     forward(sideLength);
00248     turn(90);
00249     penDown();
00250     forward(sideLength);
00251     turn(90);
00252 }
00253 
00254 void makeTriangle(int sideLength) {
00255     penDown();
00256     forward(sideLength);
00257     turn(120);
00258     penDown();
00259     forward(sideLength);
00260     turn(120);
00261     penDown();
00262     forward(sideLength);
00263 }
00264 
00265 void makeHexagon(int sideLength) {
00266     penDown();
00267     forward(sideLength);
00268     turn(60);
00269     penDown();
00270     forward(sideLength);
00271     turn(60);
00272     penDown();
00273     forward(sideLength);
00274     turn(60);
00275     penDown();
00276     forward(sideLength);
00277     turn(60);
00278     penDown();
00279     forward(sideLength);
00280     turn(60);
00281     penDown();
00282     forward(sideLength);
00283     turn(60);
00284 }
00285 
00286 void makePattern(char *shape, int sideLength){
00287     if (strcmp(shape, "triangle") == 0) {
00288         makeTriangle(sideLength);
00289         //turn(150);
00290         makeTriangle(sideLength);
00291         //turn(150);
00292         makeTriangle(sideLength);
00293         //turn(150);
00294         //makeTriangle(sideLength);
00295     } else if (strcmp(shape, "circle") == 0) {
00296         makeCircle();
00297         turn(120);
00298         makeCircle();
00299         turn(120);
00300         makeCircle();
00301         turn(120);
00302     } else if (strcmp(shape, "hexagon") == 0) {
00303         makeHexagon(sideLength);
00304         turn(120);
00305         makeHexagon(sideLength);
00306         turn(120);
00307         makeHexagon(sideLength);
00308         turn(120);
00309     } else if (strcmp(shape, "square") == 0) {
00310         makeSquare(sideLength);
00311         turn(120);
00312         makeSquare(sideLength);
00313         turn(120);
00314         makeSquare(sideLength);
00315         turn(120);
00316     }
00317 }
00318 
00319 void manualCheck() {
00320     leftEnc.reset();
00321     rightEnc.reset();
00322     while (1) {
00323         printf("Left Ticks: %d\r\n", leftEnc.read());
00324         printf("Right Ticks: %d\r\n\r\n", rightEnc.read());
00325         wait(0.5);
00326     }
00327 }
00328 
00329 // command character array for bluetooth parsing
00330 char command[100];
00331 
00332 void blue_interrupt() {
00333     // bluetooth receiver interrupt
00334     char letter = (char) blue.getc();
00335     command[strlen(command)] = letter;
00336 }
00337 
00338 void get_command() {
00339     memset(command, '\0', sizeof(command)); // resetting command buffer
00340     while (!strlen(command)) {  // waiting for full command to be read
00341         led4 = !led4;
00342         wait(1);
00343     }
00344 }
00345 
00346 Transition firstTransition(0,0,.25,.25,false);
00347 
00348 void drawLetter(Transition letter[], int numSteps) {
00349     double currAngle = 0.0;
00350     for (int i = 0; i < numSteps; i++) {
00351         double desAngle = letter[i].desired_angle();
00352         double delta = desAngle - currAngle;
00353         double distance = letter[i].distance();
00354         
00355         if (delta > 90.0) {
00356             delta -= 180;
00357             turn(delta);
00358             if (letter[i].draw == true) {
00359                 penDown();
00360             } else {
00361                 penUp();
00362             }
00363             reverse(distance);
00364         } else if (delta < -90.0) {
00365             delta += 180;
00366             turn(delta);
00367             if (letter[i].draw == true) {
00368                 penDown();
00369             } else {
00370                 penUp();
00371             }
00372             reverse(distance);
00373         } else {
00374             turn(delta);
00375             if (letter[i].draw == true) {
00376                 penDown();
00377             } else {
00378                 penUp();
00379             }
00380             forward(distance);
00381         }
00382         
00383         currAngle += delta;
00384     }
00385     double delta = 0 - currAngle;
00386     turn(delta);
00387 }
00388 
00389 void drawWord(char *word) {
00390     for (int i = 0; i < strlen(word); i++) {
00391         switch (word[i]) {
00392             case 'h':
00393                 drawLetter(let_h, 7);
00394                 break;
00395             case 'e':
00396                 drawLetter(let_e, 7);
00397                 break;
00398             case 'l':
00399                 drawLetter(let_l, 4);
00400                 break;
00401             case 'o':
00402                 drawLetter(let_o, 6);
00403                 break;
00404             default:
00405                 blue.printf("    Letter not recognized\n");
00406         }
00407     }
00408 }
00409 
00410 int main() {
00411     
00412     penDown();
00413     
00414     blue.baud(9600);
00415     blue.attach(&blue_interrupt);
00416     
00417     while (1) {
00418         get_command();
00419         
00420         const char *delimiter = " ";    // space character to separate arguments
00421         
00422         char *arg1 = strtok(command, delimiter);
00423         char *arg2 = strtok(NULL, delimiter);
00424         char *arg3 = strtok(NULL, delimiter);
00425         
00426         if (strcmp(arg1, "draw") == 0) {
00427             
00428             // first argument is draw
00429             if (strcmp(arg2, "square") == 0) {
00430                 // second argument is square
00431                 int sideDistance = 3;
00432                 if (arg3 != NULL) {
00433                     sideDistance = arg3[0] - '0';      
00434                 }
00435                 blue.printf("Drawing Square with side length = %d\n", sideDistance);
00436                 makeSquare(sideDistance);
00437             } else if (strcmp(arg2, "circle") == 0) {
00438                 // second argument is circle
00439                 blue.printf("Drawing Circle\n");
00440                 makeCircle();
00441             } else if (strcmp(arg2, "triangle") == 0) {
00442                 // second argument is triangle
00443                 int sideDistance = 3;
00444                 if (arg3 != NULL) {
00445                     sideDistance = arg3[0] - '0';      
00446                 }
00447                 blue.printf("Drawing Triangle with side length = %d\n", sideDistance);
00448                 makeTriangle(sideDistance);
00449             } else if (strcmp(arg2, "hexagon") == 0) {
00450                 // second argument is hexagon
00451                 int sideDistance = 3;
00452                 if (arg3 != NULL) {
00453                     sideDistance = arg3[0] - '0';      
00454                 }
00455                 blue.printf("Drawing Hexagon with side length = %d\n", sideDistance);
00456                 makeHexagon(sideDistance);
00457             } else {
00458                 // second argument is not recognized
00459                 blue.printf("Second argument is not recognized (must be square, circle, triangle, hexagon)\n");
00460             }
00461             
00462         } else if (strcmp(arg1, "write") == 0) {
00463             
00464             // first argument is write
00465             blue.printf("Writing the word: %s\n", arg2);
00466             drawWord(arg2);
00467             
00468         } else if (strcmp(arg1, "pattern") == 0) {
00469             
00470             // first argument is pattern
00471             if ((strcmp(arg2, "square") != 0) && (strcmp(arg2, "circle") != 0) && (strcmp(arg2, "triangle") != 0) && (strcmp(arg2, "hexagon") != 0)) {
00472                 blue.printf("ERROR: Second argument is not recognized (square, circle, triangle, hexagon)\n");
00473             } else {
00474                 int sideDistance = 3;
00475                 if (arg3 != NULL) {
00476                     sideDistance = arg3[0] - '0';
00477                 }
00478                 blue.printf("Drawing %s pattern with side length = %d\n", arg2, sideDistance);
00479                 makePattern(arg2, sideDistance);
00480             }
00481             
00482         } else {
00483             // first argument is not recognized
00484             blue.printf("ERROR: First argument is not recognized (must be draw or write)\n");
00485         }
00486         
00487         blue.printf("\n");
00488     }
00489 }