File content as of revision 4:a21d15629407:

#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 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() {
    //forward(5.0);
    //turnLeft(90);
    for (int i = 0; i < 3; i++){
        forward(5.0);
        turnLeft(120);
    }
    //manualcheck();
    //forward(10.0);
}