File content as of revision 10:2eaab13dec3c:

#include "mbed.h"
#include "EthernetInterface.h"
#include "HCSR04.h"
#include "Servo.h"

#define POS_CENTRO 1750
#define POS_MAX (POS_CENTRO + 110)
#define POS_MIN (POS_CENTRO - 110)
#define DIST_MIN 10
#define DIST_MAX 56
#define RESOL 1
#define PERIODO 10

const int ECHO_SERVER_PORT = 7;
const char* ECHO_SERVER_ADDRESS = "10.3.2.164";
const int BROADCAST_PORT = 58083;

bool conectado = false;

int  setpoint = 30;
 
Thread tcp_server;
Thread broadcast;
Thread leitura;
Thread controle;
Thread servo;

Mutex m_dist;
Mutex m_ref;
 
EthernetInterface eth;

HCSR04 usensor(D8,D9);
Servo Servo1(D7);

unsigned int dist;
 
void tcp_server_thread ();
void udp_broadcast ();
void leitura_thread();
void controle_thread();
void servo_thread();
 
int main (void) {
    //inicializa o módulo ethernet
    eth.init();
    eth.connect();
    
    //inicializa servo
    Servo1.Enable(POS_CENTRO,20000);
    
    // inicializa threads
    tcp_server.start(tcp_server_thread);
    broadcast.start(udp_broadcast);
    
    broadcast.join();
    
    leitura.start(leitura_thread);
    //servo.start(servo_thread);
    controle.start(controle_thread);
   
    while (true) {
       
    }
}
 
void tcp_server_thread () {
    printf("\nServer IP Address is %s\n", eth.getIPAddress());
   
    TCPSocketServer server;
    server.bind(ECHO_SERVER_PORT);
    server.listen();
   
    while (true) {
        printf("\nWait for new connection...\n");
        TCPSocketConnection client;
        server.accept(client);
        client.set_blocking(false, 1500); // Timeout after (1.5)s
       
        conectado = true; 
        printf("\nConnection from: %s\n", client.get_address());
        char buffer[256];
        while (true) {
            int n = client.receive(buffer, sizeof(buffer));
            if (n <= 0) break;
           
            // print received message to terminal
            buffer[n] = '\0';
            printf("Received message from Client :'%s'\n",buffer);
           
            m_ref.lock();
            sprintf (buffer, "%s", client.get_address());
            m_ref.unlock();
           
            // print sending message to terminal
            printf("Sending message to Client: '%s'\n",buffer);
           
            // Echo received message back to client
            client.send_all(buffer, n);
            if (n <= 0) break;
        }
       
        client.close();
    }
}
 
void udp_broadcast () {
    UDPSocket sock;
    sock.init();
    sock.set_broadcasting();
    
    Endpoint broadcast;
    broadcast.set_address("255.255.255.255", BROADCAST_PORT);
    
    char out_buffer[] = "diego";
    
    while (!conectado) {
        printf("Broadcasting...\n");
        sock.sendTo(broadcast, out_buffer, sizeof(out_buffer));
        Thread::wait(1000);
    }
    printf("Broadcast finalizado!");
}
 
void leitura_thread () {
    
    while (true) {
        m_dist.lock();
        dist = usensor.getCm();
        dist = dist/RESOL;
        dist = dist*RESOL;
        printf("cm:%ld\n",dist );
        if(dist>DIST_MAX) dist = DIST_MAX;
        if(dist<DIST_MIN) dist = DIST_MIN;
        m_dist.unlock();
        
        Thread::wait(2);
    }
}

void controle_thread(){

 float yant = 0;
 float xant = 0;
 float y = 0;
 float x = 0;
 float k = 20;
 float erro = 0;
 float erroant = 0;
 int motor;
 
    while(true){
  
       x = dist;
      
       erro = setpoint - x;
       y = 0.1*yant + erro*k - erroant*k*0.90;
    
       motor = y + POS_CENTRO;
         
       printf("y: %.01f ",y );
       printf("erro: %.01f ",erro );
       erroant = erro;
       xant = x;
       yant = y;
      
       if(motor>POS_MAX) motor = POS_MAX;
       if(motor<POS_MIN) motor = POS_MIN;
       Servo1.SetPosition(motor);
       printf("motor: %d\n",motor );
       
       

       Thread::wait(PERIODO); 
    }
 
 }
 
 void servo_thread(){
     //POSICAO CENTRO = 1470;
     while(1) {/*
      for (pos = POS_MIN; pos < POS_MAX; pos += 10) {
          Servo1.SetPosition(pos);  
        //printf("servo pos = %d\n", pos);
          Thread::wait(20);
      }
      for (pos = POS_MAX; pos > POS_MIN; pos -= 10) {
          Servo1.SetPosition(pos); 
          //printf("servo pos = %d\n", pos);
          Thread::wait(20); 
      }
         }*/
    
        Servo1.SetPosition(POS_CENTRO);
        Thread::wait(PERIODO); 
         }
     
     }
     
 void set_position(char []