Shadow Robot Distance Measurement with Bluetooth Code
Dependencies: 4DGL-uLCD-SE HALLFX_ENCODER Motor mbed-rtos mbed
Fork of rtos_mutex by
main.cpp
- Committer:
- vikram3
- Date:
- 2017-03-14
- Revision:
- 9:8e1702463051
- Parent:
- 8:0a2509a0b871
File content as of revision 9:8e1702463051:
#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) {} }