File content as of revision 13:294c06406d06:

#include "mbed.h"
#include "motordriver.h"
#include "HALLFX_ENCODER.h"
#include "Servo.h"
#include "letters.h"

#include <math.h>
#include <string.h>

#define SPEED 0.33
#define TICKSPERREV 390
#define DISTPERREV 8.25         // 8.25 inches per revolution
#define TicksPerDeg 2.42

#define PI 3.1415926535
#define abs(X) ((X < 0) ? -1 * X : X)
 
Motor right(p23, p6, p5, 1);    // pwm, fwd, rev
Motor left(p21, p7, p8, 1);     // pwm, fwd, rev

HALLFX_ENCODER leftEnc(p15);
HALLFX_ENCODER rightEnc(p16);

Serial blue(p13,p14);
Serial pc(USBTX, USBRX);
DigitalOut led1(LED1);
DigitalOut led4(LED4);

Servo pen(p24);

double penUpValue = 0.8;
double penDownValue = 1.0;

void penUp() {
    pen = penUpValue;
    wait(0.5);
}

void penDown() {
    pen = penDownValue;
    wait(0.5);
}

void stop() {
    right.speed(0.0);
    left.speed(0.0);
}

void forward(double distance) {
    double numRevs = distance / DISTPERREV;
    double numTicks = numRevs * TICKSPERREV;
    leftEnc.reset();
    rightEnc.reset();
    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);
}

void reverse(double distance) {
    double numRevs = distance / DISTPERREV;
    double numTicks = numRevs * TICKSPERREV;
    
    leftEnc.reset();
    rightEnc.reset();
    
    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);
}

void turnLeft(double degrees) {
    leftEnc.reset();
    rightEnc.reset();
    
    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) {
        
        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 turnRight(double degrees) {
    leftEnc.reset();
    rightEnc.reset();
    
    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) {
        
        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.5);
    if (delta > 0) {
        turnLeft(delta);
    } else {
        turnRight(-delta);
    }
    reverse(1.5);
}

void makeCircle() {
    penDown();
    leftEnc.reset();
    rightEnc.reset();
    right.speed(0.5);
    float numTicks = 2 * 360 * TicksPerDeg;
    while (rightEnc.read() < numTicks) {}
    stop();
    wait(0.1);
}

void makeSquare(int sideLength) {
    penDown();
    forward(sideLength);
    turn(90);
    penDown();
    forward(sideLength);
    turn(90);
    penDown();
    forward(sideLength);
    turn(90);
    penDown();
    forward(sideLength);
    turn(90);
}

void makeTriangle(int sideLength) {
    penDown();
    forward(sideLength);
    turn(120);
    penDown();
    forward(sideLength);
    turn(120);
    penDown();
    forward(sideLength);
}

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(char *shape, int sideLength){
    if (strcmp(shape, "triangle") == 0) {
        makeTriangle(sideLength);
        //turn(150);
        makeTriangle(sideLength);
        //turn(150);
        makeTriangle(sideLength);
        //turn(150);
        //makeTriangle(sideLength);
    } else if (strcmp(shape, "circle") == 0) {
        makeCircle();
        turn(120);
        makeCircle();
        turn(120);
        makeCircle();
        turn(120);
    } else if (strcmp(shape, "hexagon") == 0) {
        makeHexagon(sideLength);
        turn(120);
        makeHexagon(sideLength);
        turn(120);
        makeHexagon(sideLength);
        turn(120);
    } else if (strcmp(shape, "square") == 0) {
        makeSquare(sideLength);
        turn(120);
        makeSquare(sideLength);
        turn(120);
        makeSquare(sideLength);
        turn(120);
    }
}

void manualCheck() {
    leftEnc.reset();
    rightEnc.reset();
    while (1) {
        printf("Left Ticks: %d\r\n", leftEnc.read());
        printf("Right Ticks: %d\r\n\r\n", rightEnc.read());
        wait(0.5);
    }
}

// command character array for bluetooth parsing
char command[100];

void blue_interrupt() {
    // bluetooth receiver interrupt
    char letter = (char) blue.getc();
    command[strlen(command)] = letter;
}

void get_command() {
    memset(command, '\0', sizeof(command)); // resetting command buffer
    while (!strlen(command)) {  // waiting for full command to be read
        led4 = !led4;
        wait(1);
    }
}

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 {
            turn(delta);
            if (letter[i].draw == true) {
                penDown();
            } else {
                penUp();
            }
            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");
        }
    }
}

int main() {
    
    penDown();
    
    blue.baud(9600);
    blue.attach(&blue_interrupt);
    
    while (1) {
        get_command();
        
        const char *delimiter = " ";    // space character to separate arguments
        
        char *arg1 = strtok(command, delimiter);
        char *arg2 = strtok(NULL, delimiter);
        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;
                if (arg3 != NULL) {
                    sideDistance = arg3[0] - '0';      
                }
                blue.printf("Drawing Square with side length = %d\n", sideDistance);
                makeSquare(sideDistance);
            } else if (strcmp(arg2, "circle") == 0) {
                // second argument is circle
                blue.printf("Drawing Circle\n");
                makeCircle();
            } else if (strcmp(arg2, "triangle") == 0) {
                // second argument is triangle
                int sideDistance = 3;
                if (arg3 != NULL) {
                    sideDistance = arg3[0] - '0';      
                }
                blue.printf("Drawing Triangle with side length = %d\n", sideDistance);
                makeTriangle(sideDistance);
            } else if (strcmp(arg2, "hexagon") == 0) {
                // second argument is hexagon
                int sideDistance = 3;
                if (arg3 != NULL) {
                    sideDistance = arg3[0] - '0';      
                }
                blue.printf("Drawing Hexagon with side length = %d\n", sideDistance);
                makeHexagon(sideDistance);
            } else {
                // second argument is not recognized
                blue.printf("Second argument is not recognized (must be square, circle, triangle, hexagon)\n");
            }
            
        } else if (strcmp(arg1, "write") == 0) {
            
            // first argument is write
            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");
        }
        
        blue.printf("\n");
    }
}