servo and sonar
Dependencies: C12832_lcd Pulse RangeFinder Servo mbed-rtos mbed
Fork of Sprint3_Sonar by
Diff: main.cpp
- Revision:
- 9:fa6121cc0fff
- Parent:
- 8:214a95be5c54
- Child:
- 10:5a9a3a5704ef
--- a/main.cpp Thu Mar 26 15:38:49 2015 +0000 +++ b/main.cpp Fri Apr 24 10:16:05 2015 +0000 @@ -4,15 +4,18 @@ #include "RangeFinder.h" #include "Servo.h" +#define WAITT Thread::wait(50); + // Seeed ultrasound range finder -RangeFinder rf(p21, 10, 5800.0, 100000); +RangeFinder rf(p22, 10, 5800.0, 100000); BusOut myleds(LED1, LED2, LED3, LED4); C12832_LCD lcd; float sonar_data; -int base_servo_pos = 500; +int base_servo_pos = 1000; -Servo s1(p22); + +Servo s1(p21); AnalogIn ainLeft(p15); AnalogIn ainRight(p16); @@ -28,73 +31,50 @@ 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); } - Thread::wait(250); - } -} - -void led_display(void const *args) { - while (true) { - float led_m = 0.790474; - if (sonar_data <= led_m) - { - myleds = 15; - } - else if (sonar_data > led_m && sonar_data <= 2*led_m ) - { - myleds = 7; - } - else if (sonar_data > 2*led_m && sonar_data <= 3*led_m ) - { - myleds = 3; - } - else - { - myleds = 1; - } - - Thread::wait(250); - } -} - -void serial_display(void const *args) { - while (true) { - lcd.locate(0,0); - if (sonar_data == -1.0) { - printf("Timeout Error.\n"); - } else if (sonar_data > 5.0) { - printf("No object within detection range.\n"); - } else { - printf("Distance = %f m.\n", sonar_data); - } - Thread::wait(250); + WAITT; } } void base_servo(void const *args) { while (true) { - base_servoMutex.lock(); + base_servoMutex.lock(); + s1.SetPosition(base_servo_pos); base_servoMutex.unlock(); - Thread::wait(250); + WAITT; } } +float ajusting_sonardata(float data) +{ + if(data>0.3) + { + return 0.3; + } + else{ + return data; + } +} + + int main() { lcd.cls(); - s1.Enable(500,20000); + s1.Enable(1000,20000); - Thread thread1(led_display); +// Thread thread1(led_display); Thread thread2(lcd_display); - Thread thread3(serial_display); +// Thread thread3(serial_display); Thread thread4(base_servo); while (1) { sonar_data = rf.read_m(); base_servo_Intelligence.lock(); - base_servo_pos = 500 +((1000*sonar_data)/3); + base_servo_pos = 2200-((1400*ajusting_sonardata(sonar_data)/0.3)); base_servo_Intelligence.unlock(); - Thread::wait(250); + WAITT; } } \ No newline at end of file