main.cpp

#include "mbed.h"
#include "rtos.h"
#include "stdio.h"
#include "uLCD_4DGL.h"
#include "Motor.h"
#include "HALLFX_ENCODER.h"


Serial pc(USBTX, USBRX);
uLCD_4DGL uLCD(p13,p14,p11); // serial tx, serial rx, reset pin; the uLCD pins
Motor mL(p26, p18, p17); // pwm, fwd, rev
Motor mR(p25, p19, p20); // pwm, fwd, rev
Serial blue(p28, p27);
void motorDrive(const char*);
HALLFX_ENCODER encR(p22);
HALLFX_ENCODER encL(p23);
Mutex lcd_mutex;
Mutex print_mutex;

int readR, readL;

volatile const float speed = 0.4;
volatile bool forward = false;

void thread2(void const *args) {
    while (true) {
        lcd_mutex.lock();
        uLCD.cls();
        uLCD.text_width(3); //4X size text
        uLCD.text_height(3);
        uLCD.color(RED);
        uLCD.locate(1,2);
        if (forward) {
            readR = encR.read();
            int dist = readR*23/555;
            uLCD.printf("%i cm",dist);
        } else {
            int dist = 0;
            uLCD.printf("%i cm",dist);
        }
        lcd_mutex.unlock();
        Thread::wait(500);
    }
}

void thread5(void const *args) {
    char bnum = 0;
    char bhit = 0;
    while(1) {
        if (blue.readable()){
            lcd_mutex.lock();
            if (blue.getc()=='!') {
                if (blue.getc()=='B') { //button data
                    bnum = blue.getc(); //button number
                    bhit = blue.getc();
                    if ((bnum>='5')&&(bnum<='8')) //is a number button 1..4
                        printf("\n\rbnum is %c .", bnum);
                        if (bhit == '1') {
                            if (bnum == '1') {
                                motorDrive("Stop");
                            } else if (bnum == '5') {
                                motorDrive("Forward");
                            } else if (bnum == '6') {
                                motorDrive("Backward");
                            } else if (bnum == '7') {
                                motorDrive("Left");
                            } else if (bnum == '8') {
                                motorDrive("Right");
                            }
                        } else {
                            motorDrive("Stop");
                        }
                }
            }
            lcd_mutex.unlock();
        }
        Thread::wait(50);
    }
}

void motorDrive(const char * input) {
    
    if (strcmp(input, "Forward") == 0) {
        encR.reset();
        encL.reset();
        forward = true;
        mL.speed(speed);
        mR.speed(speed);
    } else if (strcmp(input, "Backward") == 0) {
        forward = false;
        mL.speed(-speed);
        mR.speed(-speed);
    } else if (strcmp(input, "Left") == 0) {
        forward = false;
        mL.speed(-speed);
        mR.speed(speed);
    } else if (strcmp(input, "Right") == 0) {
        forward = false;
        mL.speed(speed);
        mR.speed(-speed);
    } else {
        mL.speed(0.0);
        mR.speed(0.0);
    }
}

int main()
{
    printf("\n\r starting main");
    encR.reset();
    encL.reset();;
    
    uLCD.cls();
    
    Thread t2;
    Thread t5;

    t2.start(callback(thread2, (void *)"Th 2"));
    t5.start(callback(thread5, (void *)"Th 5"));
    
    
    while(1) {}
}