File content as of revision 10:9066603296bd:

#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.73

#define PI 3.14159265
#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();
    right.speed(SPEED);
    left.speed(SPEED);
    while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
    stop();
    wait(0.5);
}

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) {}
    stop();
    wait(0.5);
}

void turnLeft(double degrees) {
    leftEnc.reset();
    rightEnc.reset();
    right.speed(SPEED + 0.05);
    left.speed(-SPEED - 0.05);
    double numTicks = degrees * TicksPerDeg;
    while (leftEnc.read() < numTicks || rightEnc.read() < numTicks) {
        if (leftEnc.read() >= numTicks) {
            left.speed(0.0);
        }
        if (rightEnc.read() >= numTicks) {
            right.speed(0.0);
        }
    }
    stop();
    wait(0.5);
}

void turnRight(double degrees) {
    leftEnc.reset();
    rightEnc.reset();
    right.speed(-SPEED - 0.05);
    left.speed(SPEED + 0.05);
    double numTicks = degrees * TicksPerDeg;
    while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
    stop();
    wait(0.5);
}

void turn(double delta) {
    penUp();
    forward(1.75);
    if (delta > 0) {
        turnLeft(delta);
    } else {
        turnRight(-delta);
    }
    reverse(1.75);
    penDown();
}

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

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

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

void makeHexagon(int sideLength) {
    penDown();
    forward(sideLength);
    turn(60);
    forward(sideLength);
    turn(60);
    forward(sideLength);
    turn(60);
    forward(sideLength);
    turn(60);
    forward(sideLength);
    turn(60);
    forward(sideLength);
    turn(60);
}

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

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);
        } else {
            turnRight(-theta);
        }
        currAngle = desAngle;
        double distance = sqrt(pow(next.y - prev.y, 2) + pow(next.x - prev.x, 2));
        forward(distance);
    }
}

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("First argument is write\n");
            blue.printf("Second argument is %s\n", arg2);
        } else {
            // first argument is not recognized
            blue.printf("ERROR: First argument is not recognized (must be draw or write)\n");
        }
        
        blue.printf("\n");
    }
}