File content as of revision 8:e90d9ce42b33:

#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.32
#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);
    left.speed(-SPEED);
    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();
    blue.printf("Left Turn\n Left Encoder: %d\n Right Encoder: %d\n\n", leftEnc.read(), rightEnc.read());
    wait(0.5);
}

void turnRight(double degrees) {
    leftEnc.reset();
    rightEnc.reset();
    right.speed(-SPEED);
    left.speed(SPEED);
    double numTicks = degrees * TicksPerDeg;
    while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
    stop();
    blue.printf("Right Turn\n Left Encoder: %d\n Right Encoder: %d\n\n", leftEnc.read(), rightEnc.read());
    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() {
    leftEnc.reset();
    rightEnc.reset();
    right.speed(SPEED);
    float numTicks = 2 * 360 * TicksPerDeg;
    while (rightEnc.read() < numTicks) {}
    stop();
    wait(0.1);
}

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();
    //makeCircle();
    
    forward(5.0);
    turn(120);
    forward(5.0);
    turn(120);
    forward(5.0);
    turn(120);
    //forward(5.0);
    //turn(90);
    
    //drawLetter(let_a);
    /*
    blue.baud(9600);
    blue.attach(&blue_interrupt);
    
    while (1) {
        get_command();
        blue.printf("%s, len = %d\n", command, strlen(command));
        
        const char *delimiter = " ";    // space character to separate arguments
        char *arg1;
        char *arg2;
        
        arg1 = strtok(command, delimiter);
        arg2 = strtok(NULL, delimiter);
        
        if (strcmp(arg1, "draw") == 0) {
            // first argument is draw
            blue.printf("  First argument is draw\n");
            
            if (strcmp(arg2, "square") == 0) {
                // second argument is square
                blue.printf("  Second argument is square\n");
            } else if (strcmp(arg2, "circle") == 0) {
                // second argument is circle
                blue.printf("  Second argument is circle\n");
            } else {
                // second argument is not recognized
                blue.printf("  Second argument is not recognized (must be square or circle)\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("  First argument is not recognized (must be draw or write)\n");
        }
        
        blue.printf("\n");
    }
    */
    
    /*
    while(1) {
        pen.calibrate(0.0005, 180.0);
        for(float p=0; p<1.0; p += 0.1) {
            pen.write(p);
            wait(2);
        }
    }
    */
    
    //forward(5.0);
    //forward(5.0);
    //turnLeft(90);
    //Should make a triangle
    //for (int i = 0; i < 3; i++){
    //    forward(5.0);
    //    turnLeft(120);
    //}
    //manualcheck();
    //forward(10.0);
}