main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "stdio.h"
00004 #include "uLCD_4DGL.h"
00005 #include "Motor.h"
00006 #include "HALLFX_ENCODER.h"
00007 
00008 
00009 Serial pc(USBTX, USBRX);
00010 uLCD_4DGL uLCD(p13,p14,p11); // serial tx, serial rx, reset pin; the uLCD pins
00011 Motor mL(p26, p18, p17); // pwm, fwd, rev
00012 Motor mR(p25, p19, p20); // pwm, fwd, rev
00013 Serial blue(p28, p27);
00014 void motorDrive(const char*);
00015 HALLFX_ENCODER encR(p22);
00016 HALLFX_ENCODER encL(p23);
00017 Mutex lcd_mutex;
00018 Mutex print_mutex;
00019 
00020 int readR, readL;
00021 
00022 volatile const float speed = 0.4;
00023 volatile bool forward = false;
00024 
00025 void thread2(void const *args) {
00026     while (true) {
00027         lcd_mutex.lock();
00028         uLCD.cls();
00029         uLCD.text_width(3); //4X size text
00030         uLCD.text_height(3);
00031         uLCD.color(RED);
00032         uLCD.locate(1,2);
00033         if (forward) {
00034             readR = encR.read();
00035             int dist = readR*23/555;
00036             uLCD.printf("%i cm",dist);
00037         } else {
00038             int dist = 0;
00039             uLCD.printf("%i cm",dist);
00040         }
00041         lcd_mutex.unlock();
00042         Thread::wait(500);
00043     }
00044 }
00045 
00046 void thread5(void const *args) {
00047     char bnum = 0;
00048     char bhit = 0;
00049     while(1) {
00050         if (blue.readable()){
00051             lcd_mutex.lock();
00052             if (blue.getc()=='!') {
00053                 if (blue.getc()=='B') { //button data
00054                     bnum = blue.getc(); //button number
00055                     bhit = blue.getc();
00056                     if ((bnum>='5')&&(bnum<='8')) //is a number button 1..4
00057                         printf("\n\rbnum is %c .", bnum);
00058                         if (bhit == '1') {
00059                             if (bnum == '1') {
00060                                 motorDrive("Stop");
00061                             } else if (bnum == '5') {
00062                                 motorDrive("Forward");
00063                             } else if (bnum == '6') {
00064                                 motorDrive("Backward");
00065                             } else if (bnum == '7') {
00066                                 motorDrive("Left");
00067                             } else if (bnum == '8') {
00068                                 motorDrive("Right");
00069                             }
00070                         } else {
00071                             motorDrive("Stop");
00072                         }
00073                 }
00074             }
00075             lcd_mutex.unlock();
00076         }
00077         Thread::wait(50);
00078     }
00079 }
00080 
00081 void motorDrive(const char * input) {
00082     
00083     if (strcmp(input, "Forward") == 0) {
00084         encR.reset();
00085         encL.reset();
00086         forward = true;
00087         mL.speed(speed);
00088         mR.speed(speed);
00089     } else if (strcmp(input, "Backward") == 0) {
00090         forward = false;
00091         mL.speed(-speed);
00092         mR.speed(-speed);
00093     } else if (strcmp(input, "Left") == 0) {
00094         forward = false;
00095         mL.speed(-speed);
00096         mR.speed(speed);
00097     } else if (strcmp(input, "Right") == 0) {
00098         forward = false;
00099         mL.speed(speed);
00100         mR.speed(-speed);
00101     } else {
00102         mL.speed(0.0);
00103         mR.speed(0.0);
00104     }
00105 }
00106 
00107 int main()
00108 {
00109     printf("\n\r starting main");
00110     encR.reset();
00111     encL.reset();;
00112     
00113     uLCD.cls();
00114     
00115     Thread t2;
00116     Thread t5;
00117 
00118     t2.start(callback(thread2, (void *)"Th 2"));
00119     t5.start(callback(thread5, (void *)"Th 5"));
00120     
00121     
00122     while(1) {}
00123 }