Basic motor code

Dependencies:   Motordriver mbed HALLFX_ENCODER Servo

main.cpp

Committer:
jsterling30
Date:
2018-04-20
Revision:
2:8c27831ce9a2
Parent:
1:560118dc2f99
Child:
3:c4b0460d8886

File content as of revision 2:8c27831ce9a2:

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

#define SPEED 0.2
#define TICKSPERREV 384
#define DISTPERREV 8.25     // 8.25 inches per revolution
 
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) {}
    printf("Left Ticks End: %d\r\n", leftEnc.read());
    printf("Right Ticks End: %d\r\n", rightEnc.read()); 
    stop();
}

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