![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
servo and sonar
Dependencies: C12832_lcd Pulse RangeFinder Servo mbed-rtos mbed
Fork of Sprint3_Sonar by
main.cpp
- Committer:
- davidqpinho
- Date:
- 2015-04-24
- Revision:
- 10:5a9a3a5704ef
- Parent:
- 9:fa6121cc0fff
File content as of revision 10:5a9a3a5704ef:
#include "rtos.h" #include "mbed.h" #include "C12832_lcd.h" #include "RangeFinder.h" #include "Servo.h" #define WAITT Thread::wait(50); // Seeed ultrasound range finder RangeFinder rf(p22, 10, 5800.0, 100000); BusOut myleds(LED1, LED2, LED3, LED4); C12832_LCD lcd; float sonar_data; int base_servo_pos = 1000; float limit=2; Servo s1(p21); AnalogIn ainLeft(p15); AnalogIn ainRight(p16); Mutex base_servoMutex; Mutex base_servo_Intelligence; void lcd_display(void const *args) { while (true) { lcd.locate(0,0); if (sonar_data == -1.0) { lcd.printf("Timeout Error.\n"); } else if (sonar_data > 5.0) { lcd.printf("No object within detection range.\n"); } else { lcd.printf("Distance = %f m.\n", sonar_data); lcd.locate(1,0); lcd.printf("\nvalue = %d m.\n", base_servo_pos); } WAITT; } } void base_servo(void const *args) { while (true) { base_servoMutex.lock(); s1.SetPosition(base_servo_pos); base_servoMutex.unlock(); WAITT; } } float ajusting_sonardata(float data) { if(data>limit) { return limit; } else{ return data; } } int main() { lcd.cls(); s1.Enable(1000,20000); // Thread thread1(led_display); Thread thread2(lcd_display); // Thread thread3(serial_display); Thread thread4(base_servo); while (1) { sonar_data = rf.read_m(); base_servo_Intelligence.lock(); base_servo_pos = 800+((1400*ajusting_sonardata(sonar_data)/limit)); base_servo_Intelligence.unlock(); WAITT; } }