aaaaa
Dependencies: EthernetInterface HC-SR04 PID Servo mbed-rtos mbed
Fork of TCPEchoServer by
Revision 10:2eaab13dec3c, committed 2017-12-13
- Comitter:
- diego1332
- Date:
- Wed Dec 13 21:04:34 2017 +0000
- Parent:
- 9:eb22e90a3c03
- Child:
- 11:88cba231c882
- Commit message:
- teste;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HC-SR04.lib Wed Dec 13 21:04:34 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Nestordp/code/HC-SR04/#be89035b2c42
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Wed Dec 13 21:04:34 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Wed Dec 13 21:04:34 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/jdenkers/code/Servo/#352133517ccc
--- a/main.cpp Mon Nov 27 10:02:09 2017 +0000 +++ b/main.cpp Wed Dec 13 21:04:34 2017 +0000 @@ -1,52 +1,66 @@ #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"; - - -double ref = 20; -double temperatura; +const int BROADCAST_PORT = 58083; + +bool conectado = false; + +int setpoint = 30; Thread tcp_server; -Thread tcp_client; +Thread broadcast; Thread leitura; Thread controle; +Thread servo; -Mutex m_temp; +Mutex m_dist; Mutex m_ref; EthernetInterface eth; + +HCSR04 usensor(D8,D9); +Servo Servo1(D7); + +unsigned int dist; void tcp_server_thread (); -void tcp_client_thread (); -void tcp_client_send_temp (); +void udp_broadcast (); void leitura_thread(); void controle_thread(); - -AnalogIn LM35(A0); -PwmOut PWM1(A5); +void servo_thread(); int main (void) { //inicializa o módulo ethernet eth.init(); eth.connect(); - - //inicializa o PWM - PWM1.period_ms(100); - - //para não enviar a temperatura - tcp_client.signal_clr(0x1); - - /** INICIALIZAÇÃO DAS THREADS **/ + + //inicializa servo + Servo1.Enable(POS_CENTRO,20000); + + // inicializa threads tcp_server.start(tcp_server_thread); - tcp_client.start(tcp_client_thread); + broadcast.start(udp_broadcast); + + broadcast.join(); + leitura.start(leitura_thread); + //servo.start(servo_thread); controle.start(controle_thread); while (true) { - + } } @@ -63,6 +77,7 @@ 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) { @@ -74,8 +89,7 @@ printf("Received message from Client :'%s'\n",buffer); m_ref.lock(); - ref = atof(buffer); - sprintf (buffer, "%f", ref); + sprintf (buffer, "%s", client.get_address()); m_ref.unlock(); // print sending message to terminal @@ -90,87 +104,97 @@ } } -void tcp_client_thread () { - printf("\nClient IP Address is %s\n", eth.getIPAddress()); - - while(true) { - Thread::signal_wait(0x1); - tcp_client_send_temp(); - } -} - -void tcp_client_send_temp () { - // Connect to Server - TCPSocketConnection socket; - while (socket.connect(ECHO_SERVER_ADDRESS, 5000) < 0) { - printf("Unable to connect to (%s) on port (%d)\n", ECHO_SERVER_ADDRESS, 5000); +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("Connected to Server at %s\n",ECHO_SERVER_ADDRESS); - - // Send message to server - char buffer[10]; - int num; - - m_temp.lock(); - num = sprintf (buffer, "%.1f", temperatura); - m_temp.unlock(); - - //printf("Sending message to Server : '%s' \n",buffer); - socket.send_all(buffer, num); - - // Receive message from server - char buf[256]; - int n = socket.receive(buf, 256); - buf[n] = '\0'; - //printf("Received message from server: '%s'\n", buf); - - // Clean up - socket.close(); + printf("Broadcast finalizado!"); } void leitura_thread () { - double meas; + while (true) { - meas = LM35.read(); // Converts and read the analog input value (value from 0.0 to 1.0) + 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(); - m_temp.lock(); - temperatura = meas * 330; // (0,1) * 3.3 (para converter pra tensão) / 10mV por grau celsius pra temperatura - m_temp.unlock(); - - tcp_client.signal_set(0x1); //AVISA TCP_CLIENT Q PODE ENVIAR SOCKET - - Thread::wait(1000); + Thread::wait(2); } } void controle_thread(){ - int tempo = 0; - - double deltaVo, ref_tensao, erro, saidaatual, saidaantiga = 0; - + + 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){ - deltaVo = temperatura * 0.01; // para converter para tensão - deltaVo = deltaVo - 0.26; // para virar delta - - m_ref.lock(); - printf("ref = %.1f\n", ref); - ref_tensao = ref * 0.01 - 0.26; // 10mV/grau - m_ref.unlock(); - - erro = ref_tensao - deltaVo; //calcula o erro - saidaatual = 625 * erro + 0.5 * saidaantiga; // em tensão - saidaantiga = saidaatual; - saidaatual = saidaatual / 3.3; // converte para duty cycle - PWM1.write(saidaatual); - - m_temp.lock(); - printf("temp(%d) = %.1f graus\n", tempo, temperatura); - m_temp.unlock(); - - tempo++; - Thread::wait(1000); // 1s + + 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); } } - \ No newline at end of file + + 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 [] \ No newline at end of file