main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "Servo.h"
00004 
00005 Serial pc(USBTX, USBRX);
00006 
00007 DigitalOut trig(D3);
00008 DigitalIn echo(D2);
00009 Servo myservo(D4);
00010 
00011 Thread hilo_tarea1;
00012 Thread hilo_tarea2;
00013 Timer senal;
00014 float distancia;
00015 float angle;
00016 void tarea1(void);
00017 void tarea2(void);
00018 
00019 int main()
00020 {
00021 
00022     hilo_tarea1.start(tarea1);
00023     hilo_tarea2.start(tarea2);
00024     hilo_tarea1.set_priority(osPriorityNormal);
00025     while (true) {
00026 
00027     }
00028 }
00029 
00030 void tarea1(void)
00031 {
00032     while(true) {
00033         senal.reset();
00034         trig = 0;
00035         wait_us(2);
00036         trig = 1;
00037         wait_us(10);
00038         trig = 0;
00039         while(echo==0);
00040         senal.start();
00041         while(echo==1);
00042         senal.stop();
00043         distancia = (senal.read_us())/58;
00044         pc.printf("%.f;%.f\n\r",angle,distancia);
00045         ThisThread::sleep_for(50);
00046     }
00047 }
00048 
00049 void tarea2(void)
00050 {
00051     float range = 0.0009;
00052     bool direction = true;
00053     float position = 0.0;
00054     myservo.calibrate(range, 45.0);
00055     while(true) {
00056         if(direction) {
00057             for (int i=-90; i< 90; i++) {
00058                 angle= i+90;
00059                 myservo.position(i);
00060                 wait_ms(17);
00061             }
00062             direction=false;
00063         } else {
00064             for (int i=90; i> -90; i--) {
00065                 angle = i+90;
00066                 myservo.position(i);
00067                 wait_ms(17);
00068             }
00069             direction=true;
00070 
00071         }
00072     }
00073 }