File content as of revision 3:c4b0460d8886:

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

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 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() {
    //forward(5.0);
    //turnLeft(90);
    forward(5.0);
    turnLeft(45);
    forward(5.0);
    //manualcheck();
    //forward(10.0);
}