Diff: main.cpp

Revision:

11:88cba231c882

Parent:

10:2eaab13dec3c

--- a/main.cpp	Wed Dec 13 21:04:34 2017 +0000
+++ b/main.cpp	Thu Dec 14 00:10:14 2017 +0000
@@ -2,6 +2,7 @@
 #include "EthernetInterface.h"
 #include "HCSR04.h"
 #include "Servo.h"
+#include <string.h>
 
 #define POS_CENTRO 1750
 #define POS_MAX (POS_CENTRO + 110)
@@ -17,184 +18,181 @@
 
 bool conectado = false;
 
-int  setpoint = 30;
- 
+int  setpoint;
+
 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) {
+
+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);
-   
+    printf("Todas as thread iniciadas\n");
+
     while (true) {
-       
+
     }
 }
- 
-void tcp_server_thread () {
+
+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; 
+
+        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;
+            //client.send_all(buffer, n);
+           // if (n <= 0) break;
+           memset(buffer,0,256);
         }
-       
+
         client.close();
     }
 }
- 
-void udp_broadcast () {
+
+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!");
+    printf("Broadcast finalizado!\n");
 }
- 
-void leitura_thread () {
-    
+
+void leitura_thread ()
+{
+
     while (true) {
         m_dist.lock();
         dist = usensor.getCm();
         dist = dist/RESOL;
         dist = dist*RESOL;
-        printf("cm:%ld\n",dist );
+    //  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(){
+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;
 
- 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 );
-       
-       
+        erro = setpoint - x;
+        
+        m_dist.unlock();
+        
+        y = 0.1*yant + erro*k - erroant*k*0.90;
+
+        motor = y + POS_CENTRO;
 
-       Thread::wait(PERIODO); 
+        //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 [] 
\ No newline at end of file
+
+}