Diff: main.cpp

Revision:

10:2eaab13dec3c

Parent:

9:eb22e90a3c03

Child:

11:88cba231c882

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