Sprint One / Mbed 2 deprecated Sprint3_Sonar2

servo and sonar

Dependencies:   C12832_lcd Pulse RangeFinder Servo mbed-rtos mbed

Fork of Sprint3_Sonar by Sprint One

main.cpp@9:fa6121cc0fff, 2015-04-24 (annotated)

Committer:
davidqpinho
Date:
Fri Apr 24 10:16:05 2015 +0000
Revision:
9:fa6121cc0fff
Parent:
8:214a95be5c54
Child:
10:5a9a3a5704ef
sonar

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hervel90 7:cb446dbb54d9 1 #include "rtos.h"
emilmont 1:491820ee784d 2 #include "mbed.h"
hervel90 7:cb446dbb54d9 3 #include "C12832_lcd.h"
hervel90 7:cb446dbb54d9 4 #include "RangeFinder.h"
davidqpinho 8:214a95be5c54 5 #include "Servo.h"
hervel90 7:cb446dbb54d9 6
davidqpinho 9:fa6121cc0fff 7 #define WAITT Thread::wait(50);
davidqpinho 9:fa6121cc0fff 8
hervel90 7:cb446dbb54d9 9 // Seeed ultrasound range finder
davidqpinho 9:fa6121cc0fff 10 RangeFinder rf(p22, 10, 5800.0, 100000);
hervel90 7:cb446dbb54d9 11 BusOut myleds(LED1, LED2, LED3, LED4);
hervel90 7:cb446dbb54d9 12 C12832_LCD lcd;
hervel90 7:cb446dbb54d9 13
hervel90 7:cb446dbb54d9 14 float sonar_data;
davidqpinho 9:fa6121cc0fff 15 int base_servo_pos = 1000;
hervel90 7:cb446dbb54d9 16
davidqpinho 9:fa6121cc0fff 17
davidqpinho 9:fa6121cc0fff 18 Servo s1(p21);
davidqpinho 8:214a95be5c54 19 AnalogIn ainLeft(p15);
davidqpinho 8:214a95be5c54 20 AnalogIn ainRight(p16);
davidqpinho 8:214a95be5c54 21
davidqpinho 8:214a95be5c54 22 Mutex base_servoMutex;
davidqpinho 8:214a95be5c54 23 Mutex base_servo_Intelligence;
hervel90 7:cb446dbb54d9 24
hervel90 7:cb446dbb54d9 25 void lcd_display(void const *args) {
emilmont 1:491820ee784d 26 while (true) {
hervel90 7:cb446dbb54d9 27 lcd.locate(0,0);
hervel90 7:cb446dbb54d9 28 if (sonar_data == -1.0) {
hervel90 7:cb446dbb54d9 29 lcd.printf("Timeout Error.\n");
hervel90 7:cb446dbb54d9 30 } else if (sonar_data > 5.0) {
hervel90 7:cb446dbb54d9 31 lcd.printf("No object within detection range.\n");
hervel90 7:cb446dbb54d9 32 } else {
hervel90 7:cb446dbb54d9 33 lcd.printf("Distance = %f m.\n", sonar_data);
davidqpinho 9:fa6121cc0fff 34 lcd.locate(1,0);
davidqpinho 9:fa6121cc0fff 35 lcd.printf("\nvalue = %d m.\n", base_servo_pos);
hervel90 7:cb446dbb54d9 36 }
davidqpinho 9:fa6121cc0fff 37 WAITT;
hervel90 7:cb446dbb54d9 38 }
hervel90 7:cb446dbb54d9 39 }
hervel90 7:cb446dbb54d9 40
davidqpinho 8:214a95be5c54 41 void base_servo(void const *args) {
davidqpinho 8:214a95be5c54 42 while (true) {
davidqpinho 9:fa6121cc0fff 43 base_servoMutex.lock();
davidqpinho 9:fa6121cc0fff 44
davidqpinho 8:214a95be5c54 45 s1.SetPosition(base_servo_pos);
davidqpinho 8:214a95be5c54 46 base_servoMutex.unlock();
davidqpinho 9:fa6121cc0fff 47 WAITT;
davidqpinho 8:214a95be5c54 48 }
davidqpinho 8:214a95be5c54 49 }
hervel90 7:cb446dbb54d9 50
davidqpinho 9:fa6121cc0fff 51 float ajusting_sonardata(float data)
davidqpinho 9:fa6121cc0fff 52 {
davidqpinho 9:fa6121cc0fff 53 if(data>0.3)
davidqpinho 9:fa6121cc0fff 54 {
davidqpinho 9:fa6121cc0fff 55 return 0.3;
davidqpinho 9:fa6121cc0fff 56 }
davidqpinho 9:fa6121cc0fff 57 else{
davidqpinho 9:fa6121cc0fff 58 return data;
davidqpinho 9:fa6121cc0fff 59 }
davidqpinho 9:fa6121cc0fff 60 }
davidqpinho 9:fa6121cc0fff 61
davidqpinho 9:fa6121cc0fff 62
hervel90 7:cb446dbb54d9 63 int main() {
hervel90 7:cb446dbb54d9 64 lcd.cls();
hervel90 7:cb446dbb54d9 65
davidqpinho 9:fa6121cc0fff 66 s1.Enable(1000,20000);
davidqpinho 8:214a95be5c54 67
davidqpinho 9:fa6121cc0fff 68 // Thread thread1(led_display);
hervel90 7:cb446dbb54d9 69 Thread thread2(lcd_display);
davidqpinho 9:fa6121cc0fff 70 // Thread thread3(serial_display);
davidqpinho 8:214a95be5c54 71 Thread thread4(base_servo);
hervel90 7:cb446dbb54d9 72
hervel90 7:cb446dbb54d9 73 while (1) {
hervel90 7:cb446dbb54d9 74 sonar_data = rf.read_m();
davidqpinho 8:214a95be5c54 75 base_servo_Intelligence.lock();
davidqpinho 9:fa6121cc0fff 76 base_servo_pos = 2200-((1400*ajusting_sonardata(sonar_data)/0.3));
davidqpinho 8:214a95be5c54 77 base_servo_Intelligence.unlock();
davidqpinho 9:fa6121cc0fff 78 WAITT;
hervel90 7:cb446dbb54d9 79 }
hervel90 7:cb446dbb54d9 80 }