File content as of revision 11:88cba231c882:

#include "mbed.h"
#include "EthernetInterface.h"
#include "HCSR04.h"
#include "Servo.h"
#include <string.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;

Thread tcp_server;
Thread broadcast;
Thread leitura;
Thread controle;

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();

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);
    controle.start(controle_thread);
    printf("Todas as thread iniciadas\n");

    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);

            int pos =  buffer[8] - '0';

            if(pos == 1)setpoint = 20;
            else if(pos == 2)setpoint = 35;
            else if(pos == 3)setpoint = 50;
            printf("Setpoint: %d\n", setpoint);

            m_ref.lock();
            // ref = atof(buffer);
            sprintf (buffer, "%s", client.get_address());
            m_ref.unlock();

            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;
           memset(buffer,0,256);
        }

        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!\n");
}

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 = 60;
    float erro = 0;
    float erroant = 0;
    int motor;

    while(true) {
        
        m_dist.lock();
        x = dist;

        erro = setpoint - x;
        
        m_dist.unlock();
        
        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);
    }

}