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@10:5a9a3a5704ef, 2015-04-24 (annotated)

Committer:
davidqpinho
Date:
Fri Apr 24 10:44:25 2015 +0000
Revision:
10:5a9a3a5704ef
Parent:
9:fa6121cc0fff 
dsadsa

;

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;
davidqpinho 10:5a9a3a5704ef 16 float limit=2;
hervel90 7:cb446dbb54d9 17
davidqpinho 9:fa6121cc0fff 18
davidqpinho 9:fa6121cc0fff 19 Servo s1(p21);
davidqpinho 8:214a95be5c54 20 AnalogIn ainLeft(p15);
davidqpinho 8:214a95be5c54 21 AnalogIn ainRight(p16);
davidqpinho 8:214a95be5c54 22
davidqpinho 8:214a95be5c54 23 Mutex base_servoMutex;
davidqpinho 8:214a95be5c54 24 Mutex base_servo_Intelligence;
hervel90 7:cb446dbb54d9 25
hervel90 7:cb446dbb54d9 26 void lcd_display(void const *args) {
emilmont 1:491820ee784d 27 while (true) {
hervel90 7:cb446dbb54d9 28 lcd.locate(0,0);
hervel90 7:cb446dbb54d9 29 if (sonar_data == -1.0) {
hervel90 7:cb446dbb54d9 30 lcd.printf("Timeout Error.\n");
hervel90 7:cb446dbb54d9 31 } else if (sonar_data > 5.0) {
hervel90 7:cb446dbb54d9 32 lcd.printf("No object within detection range.\n");
hervel90 7:cb446dbb54d9 33 } else {
hervel90 7:cb446dbb54d9 34 lcd.printf("Distance = %f m.\n", sonar_data);
davidqpinho 9:fa6121cc0fff 35 lcd.locate(1,0);
davidqpinho 9:fa6121cc0fff 36 lcd.printf("\nvalue = %d m.\n", base_servo_pos);
hervel90 7:cb446dbb54d9 37 }
davidqpinho 9:fa6121cc0fff 38 WAITT;
hervel90 7:cb446dbb54d9 39 }
hervel90 7:cb446dbb54d9 40 }
hervel90 7:cb446dbb54d9 41
davidqpinho 8:214a95be5c54 42 void base_servo(void const *args) {
davidqpinho 8:214a95be5c54 43 while (true) {
davidqpinho 9:fa6121cc0fff 44 base_servoMutex.lock();
davidqpinho 9:fa6121cc0fff 45
davidqpinho 8:214a95be5c54 46 s1.SetPosition(base_servo_pos);
davidqpinho 8:214a95be5c54 47 base_servoMutex.unlock();
davidqpinho 9:fa6121cc0fff 48 WAITT;
davidqpinho 8:214a95be5c54 49 }
davidqpinho 8:214a95be5c54 50 }
hervel90 7:cb446dbb54d9 51
davidqpinho 9:fa6121cc0fff 52 float ajusting_sonardata(float data)
davidqpinho 9:fa6121cc0fff 53 {
davidqpinho 10:5a9a3a5704ef 54 if(data>limit)
davidqpinho 9:fa6121cc0fff 55 {
davidqpinho 10:5a9a3a5704ef 56 return limit;
davidqpinho 9:fa6121cc0fff 57 }
davidqpinho 9:fa6121cc0fff 58 else{
davidqpinho 9:fa6121cc0fff 59 return data;
davidqpinho 9:fa6121cc0fff 60 }
davidqpinho 9:fa6121cc0fff 61 }
davidqpinho 9:fa6121cc0fff 62
davidqpinho 9:fa6121cc0fff 63
hervel90 7:cb446dbb54d9 64 int main() {
hervel90 7:cb446dbb54d9 65 lcd.cls();
hervel90 7:cb446dbb54d9 66
davidqpinho 9:fa6121cc0fff 67 s1.Enable(1000,20000);
davidqpinho 8:214a95be5c54 68
davidqpinho 9:fa6121cc0fff 69 // Thread thread1(led_display);
hervel90 7:cb446dbb54d9 70 Thread thread2(lcd_display);
davidqpinho 9:fa6121cc0fff 71 // Thread thread3(serial_display);
davidqpinho 8:214a95be5c54 72 Thread thread4(base_servo);
hervel90 7:cb446dbb54d9 73
hervel90 7:cb446dbb54d9 74 while (1) {
hervel90 7:cb446dbb54d9 75 sonar_data = rf.read_m();
davidqpinho 8:214a95be5c54 76 base_servo_Intelligence.lock();
davidqpinho 10:5a9a3a5704ef 77 base_servo_pos = 800+((1400*ajusting_sonardata(sonar_data)/limit));
davidqpinho 8:214a95be5c54 78 base_servo_Intelligence.unlock();
davidqpinho 9:fa6121cc0fff 79 WAITT;
hervel90 7:cb446dbb54d9 80 }
hervel90 7:cb446dbb54d9 81 }