File content as of revision 5:7572f73a78f3:

#include "mbed.h"
#include "motordriver.h"
#include "HALLFX_ENCODER.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);

RawSerial  pc(USBTX, USBRX);
RawSerial  dev(p28,p27);
DigitalOut led1(LED1);
DigitalOut led4(LED4);

void dev_recv()
{
    led1 = !led1;
    while(dev.readable()) {
        pc.putc(dev.getc());
    }
}

void pc_recv()
{
    led4 = !led4;
    while(pc.readable()) {
        dev.putc(pc.getc());
    }
}

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

int main() {
    pc.baud(9600);
    dev.baud(9600);

    pc.attach(&pc_recv, Serial::RxIrq);
    dev.attach(&dev_recv, Serial::RxIrq);

    while(1) {
        sleep();
    }
    //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);
}