Basic motor code

Dependencies:   Motordriver mbed HALLFX_ENCODER Servo

main.cpp

Committer:
jsterling30
Date:
2018-04-21
Revision:
6:235548599e79
Parent:
5:7572f73a78f3
Child:
7:6ef9b0aab972

File content as of revision 6:235548599e79:

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

#include <string>

#define SPEED 0.2
#define TICKSPERREV 390
#define DISTPERREV 8.25     // 8.25 inches per revolution
#define TicksPerDeg 2.756

using namespace std;
 
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 dev(p13,p14);
RawSerial pc(USBTX, USBRX);
DigitalOut led1(LED1);
DigitalOut led4(LED4);

int count = 0;

void dev_recv()
{
    count++;
    led1 = !led1;
    while(dev.readable()) {
        pc.putc(dev.getc());
    }
    pc.printf("%d", count);
}
 
void pc_recv()
{
    led4 = !led4;
    while(pc.readable()) {
        dev.putc(pc.getc());
    }
}

string command;

void blue_recv()
{
    led1 = !led1;
    while(dev.readable()) {
        char letter = (char) dev.getc();
        command += letter;
        //strcat(command, &letter);
        //pc.putc(dev.getc());
    }
    pc.printf("%s", command);
    
    string circle = "circle";
    if (circle.compare(0,6,command) == 0) {
        dev.puts("Recognized word circle");
    } else {
        dev.puts("Did not recognize command");
    }
}

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(&blue_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);
}