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@8:214a95be5c54, 2015-03-26 (annotated)

Committer:
davidqpinho
Date:
Thu Mar 26 15:38:49 2015 +0000
Revision:
8:214a95be5c54
Parent:
7:cb446dbb54d9
Child:
9:fa6121cc0fff
base servo code

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
hervel90 7:cb446dbb54d9 7 // Seeed ultrasound range finder
hervel90 7:cb446dbb54d9 8 RangeFinder rf(p21, 10, 5800.0, 100000);
hervel90 7:cb446dbb54d9 9 BusOut myleds(LED1, LED2, LED3, LED4);
hervel90 7:cb446dbb54d9 10 C12832_LCD lcd;
hervel90 7:cb446dbb54d9 11
hervel90 7:cb446dbb54d9 12 float sonar_data;
davidqpinho 8:214a95be5c54 13 int base_servo_pos = 500;
hervel90 7:cb446dbb54d9 14
davidqpinho 8:214a95be5c54 15 Servo s1(p22);
davidqpinho 8:214a95be5c54 16 AnalogIn ainLeft(p15);
davidqpinho 8:214a95be5c54 17 AnalogIn ainRight(p16);
davidqpinho 8:214a95be5c54 18
davidqpinho 8:214a95be5c54 19 Mutex base_servoMutex;
davidqpinho 8:214a95be5c54 20 Mutex base_servo_Intelligence;
hervel90 7:cb446dbb54d9 21
hervel90 7:cb446dbb54d9 22 void lcd_display(void const *args) {
emilmont 1:491820ee784d 23 while (true) {
hervel90 7:cb446dbb54d9 24 lcd.locate(0,0);
hervel90 7:cb446dbb54d9 25 if (sonar_data == -1.0) {
hervel90 7:cb446dbb54d9 26 lcd.printf("Timeout Error.\n");
hervel90 7:cb446dbb54d9 27 } else if (sonar_data > 5.0) {
hervel90 7:cb446dbb54d9 28 lcd.printf("No object within detection range.\n");
hervel90 7:cb446dbb54d9 29 } else {
hervel90 7:cb446dbb54d9 30 lcd.printf("Distance = %f m.\n", sonar_data);
hervel90 7:cb446dbb54d9 31 }
hervel90 7:cb446dbb54d9 32 Thread::wait(250);
emilmont 1:491820ee784d 33 }
emilmont 1:491820ee784d 34 }
hervel90 7:cb446dbb54d9 35
hervel90 7:cb446dbb54d9 36 void led_display(void const *args) {
hervel90 7:cb446dbb54d9 37 while (true) {
hervel90 7:cb446dbb54d9 38 float led_m = 0.790474;
hervel90 7:cb446dbb54d9 39 if (sonar_data <= led_m)
hervel90 7:cb446dbb54d9 40 {
hervel90 7:cb446dbb54d9 41 myleds = 15;
hervel90 7:cb446dbb54d9 42 }
hervel90 7:cb446dbb54d9 43 else if (sonar_data > led_m && sonar_data <= 2*led_m )
hervel90 7:cb446dbb54d9 44 {
hervel90 7:cb446dbb54d9 45 myleds = 7;
hervel90 7:cb446dbb54d9 46 }
hervel90 7:cb446dbb54d9 47 else if (sonar_data > 2*led_m && sonar_data <= 3*led_m )
hervel90 7:cb446dbb54d9 48 {
hervel90 7:cb446dbb54d9 49 myleds = 3;
hervel90 7:cb446dbb54d9 50 }
hervel90 7:cb446dbb54d9 51 else
hervel90 7:cb446dbb54d9 52 {
hervel90 7:cb446dbb54d9 53 myleds = 1;
hervel90 7:cb446dbb54d9 54 }
emilmont 1:491820ee784d 55
hervel90 7:cb446dbb54d9 56 Thread::wait(250);
emilmont 1:491820ee784d 57 }
emilmont 1:491820ee784d 58 }
hervel90 7:cb446dbb54d9 59
hervel90 7:cb446dbb54d9 60 void serial_display(void const *args) {
hervel90 7:cb446dbb54d9 61 while (true) {
hervel90 7:cb446dbb54d9 62 lcd.locate(0,0);
hervel90 7:cb446dbb54d9 63 if (sonar_data == -1.0) {
hervel90 7:cb446dbb54d9 64 printf("Timeout Error.\n");
hervel90 7:cb446dbb54d9 65 } else if (sonar_data > 5.0) {
hervel90 7:cb446dbb54d9 66 printf("No object within detection range.\n");
hervel90 7:cb446dbb54d9 67 } else {
hervel90 7:cb446dbb54d9 68 printf("Distance = %f m.\n", sonar_data);
hervel90 7:cb446dbb54d9 69 }
hervel90 7:cb446dbb54d9 70 Thread::wait(250);
hervel90 7:cb446dbb54d9 71 }
hervel90 7:cb446dbb54d9 72 }
hervel90 7:cb446dbb54d9 73
davidqpinho 8:214a95be5c54 74 void base_servo(void const *args) {
davidqpinho 8:214a95be5c54 75 while (true) {
davidqpinho 8:214a95be5c54 76 base_servoMutex.lock();
davidqpinho 8:214a95be5c54 77 s1.SetPosition(base_servo_pos);
davidqpinho 8:214a95be5c54 78 base_servoMutex.unlock();
davidqpinho 8:214a95be5c54 79 Thread::wait(250);
davidqpinho 8:214a95be5c54 80 }
davidqpinho 8:214a95be5c54 81 }
hervel90 7:cb446dbb54d9 82
hervel90 7:cb446dbb54d9 83 int main() {
hervel90 7:cb446dbb54d9 84 lcd.cls();
hervel90 7:cb446dbb54d9 85
davidqpinho 8:214a95be5c54 86 s1.Enable(500,20000);
davidqpinho 8:214a95be5c54 87
hervel90 7:cb446dbb54d9 88 Thread thread1(led_display);
hervel90 7:cb446dbb54d9 89 Thread thread2(lcd_display);
hervel90 7:cb446dbb54d9 90 Thread thread3(serial_display);
davidqpinho 8:214a95be5c54 91 Thread thread4(base_servo);
hervel90 7:cb446dbb54d9 92
hervel90 7:cb446dbb54d9 93 while (1) {
hervel90 7:cb446dbb54d9 94 sonar_data = rf.read_m();
davidqpinho 8:214a95be5c54 95 base_servo_Intelligence.lock();
davidqpinho 8:214a95be5c54 96 base_servo_pos = 500 +((1000*sonar_data)/3);
davidqpinho 8:214a95be5c54 97 base_servo_Intelligence.unlock();
hervel90 7:cb446dbb54d9 98 Thread::wait(250);
hervel90 7:cb446dbb54d9 99 }
hervel90 7:cb446dbb54d9 100 }