File content as of revision 7:6ef9b0aab972:

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

#include <string.h>

#define SPEED 0.2
#define TICKSPERREV 390
#define DISTPERREV 8.25         // 8.25 inches per revolution
#define TicksPerDeg 2.756
 
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);

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

void forward(float distance) {
    float numRevs = distance / DISTPERREV;
    float numTicks = numRevs * TICKSPERREV;
    leftEnc.reset();
    rightEnc.reset();
    printf("Left Ticks Start: %d\r\n", leftEnc.read());
    printf("Right Ticks Start: %d\r\n", rightEnc.read());
    right.speed(SPEED);
    left.speed(SPEED);
    while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
    stop();
    wait(0.1);
    printf("Left Ticks End: %d\r\n", leftEnc.read());
    printf("Right Ticks End: %d\r\n", rightEnc.read());
}

void turnLeft(int degrees) {
    leftEnc.reset();
    rightEnc.reset();
    right.speed(SPEED);
    left.speed(-SPEED);
    float numTicks = degrees * TicksPerDeg;
    while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
    stop();
    wait(0.1);
}

void turnRight(int degrees) {
    leftEnc.reset();
    rightEnc.reset();
    right.speed(-SPEED);
    left.speed(SPEED);
    float numTicks = degrees * TicksPerDeg;
    while (leftEnc.read() < numTicks && rightEnc.read() < numTicks) {}
    stop();
    wait(0.1);
}

void makeCircle() {
    leftEnc.reset();
    rightEnc.reset();
    right.speed(SPEED);
    float numTicks = 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);
    }
}

int main() {
    
    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);
}