Diego Carrillo / Mbed 2 deprecated ProjetoSO

aaaaa

Dependencies:   EthernetInterface HC-SR04 PID Servo mbed-rtos mbed

Fork of TCPEchoServer by Paulo Ludewig

main.cpp@11:88cba231c882, 2017-12-14 (annotated)

Committer:
diego1332
Date:
Thu Dec 14 00:10:14 2017 +0000
Revision:
11:88cba231c882
Parent:
10:2eaab13dec3c 
vers?o final;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbed_official 0:38cbb854d85f 1 #include "mbed.h"
emilmont 1:5cebe0e38cd2 2 #include "EthernetInterface.h"
diego1332 10:2eaab13dec3c 3 #include "HCSR04.h"
diego1332 10:2eaab13dec3c 4 #include "Servo.h"
diego1332 11:88cba231c882 5 #include <string.h>
pauloludewig 9:eb22e90a3c03 6
diego1332 10:2eaab13dec3c 7 #define POS_CENTRO 1750
diego1332 10:2eaab13dec3c 8 #define POS_MAX (POS_CENTRO + 110)
diego1332 10:2eaab13dec3c 9 #define POS_MIN (POS_CENTRO - 110)
diego1332 10:2eaab13dec3c 10 #define DIST_MIN 10
diego1332 10:2eaab13dec3c 11 #define DIST_MAX 56
diego1332 10:2eaab13dec3c 12 #define RESOL 1
diego1332 10:2eaab13dec3c 13 #define PERIODO 10
diego1332 10:2eaab13dec3c 14
pauloludewig 9:eb22e90a3c03 15 const int ECHO_SERVER_PORT = 7;
pauloludewig 9:eb22e90a3c03 16 const char* ECHO_SERVER_ADDRESS = "10.3.2.164";
diego1332 10:2eaab13dec3c 17 const int BROADCAST_PORT = 58083;
diego1332 10:2eaab13dec3c 18
diego1332 10:2eaab13dec3c 19 bool conectado = false;
diego1332 10:2eaab13dec3c 20
diego1332 11:88cba231c882 21 int setpoint;
diego1332 11:88cba231c882 22
pauloludewig 9:eb22e90a3c03 23 Thread tcp_server;
diego1332 10:2eaab13dec3c 24 Thread broadcast;
pauloludewig 9:eb22e90a3c03 25 Thread leitura;
pauloludewig 9:eb22e90a3c03 26 Thread controle;
pauloludewig 9:eb22e90a3c03 27
diego1332 10:2eaab13dec3c 28 Mutex m_dist;
pauloludewig 9:eb22e90a3c03 29 Mutex m_ref;
diego1332 11:88cba231c882 30
pauloludewig 9:eb22e90a3c03 31 EthernetInterface eth;
diego1332 10:2eaab13dec3c 32
diego1332 10:2eaab13dec3c 33 HCSR04 usensor(D8,D9);
diego1332 10:2eaab13dec3c 34 Servo Servo1(D7);
diego1332 10:2eaab13dec3c 35
diego1332 10:2eaab13dec3c 36 unsigned int dist;
diego1332 11:88cba231c882 37
pauloludewig 9:eb22e90a3c03 38 void tcp_server_thread ();
diego1332 10:2eaab13dec3c 39 void udp_broadcast ();
pauloludewig 9:eb22e90a3c03 40 void leitura_thread();
pauloludewig 9:eb22e90a3c03 41 void controle_thread();
diego1332 11:88cba231c882 42
diego1332 11:88cba231c882 43 int main (void)
diego1332 11:88cba231c882 44 {
pauloludewig 9:eb22e90a3c03 45 //inicializa o módulo ethernet
pauloludewig 9:eb22e90a3c03 46 eth.init();
emilmont 1:5cebe0e38cd2 47 eth.connect();
diego1332 11:88cba231c882 48
diego1332 10:2eaab13dec3c 49 //inicializa servo
diego1332 10:2eaab13dec3c 50 Servo1.Enable(POS_CENTRO,20000);
diego1332 11:88cba231c882 51
diego1332 10:2eaab13dec3c 52 // inicializa threads
pauloludewig 9:eb22e90a3c03 53 tcp_server.start(tcp_server_thread);
diego1332 10:2eaab13dec3c 54 broadcast.start(udp_broadcast);
diego1332 11:88cba231c882 55
diego1332 10:2eaab13dec3c 56 broadcast.join();
diego1332 11:88cba231c882 57
pauloludewig 9:eb22e90a3c03 58 leitura.start(leitura_thread);
pauloludewig 9:eb22e90a3c03 59 controle.start(controle_thread);
diego1332 11:88cba231c882 60 printf("Todas as thread iniciadas\n");
diego1332 11:88cba231c882 61
pauloludewig 9:eb22e90a3c03 62 while (true) {
diego1332 11:88cba231c882 63
pauloludewig 9:eb22e90a3c03 64 }
pauloludewig 9:eb22e90a3c03 65 }
diego1332 11:88cba231c882 66
diego1332 11:88cba231c882 67 void tcp_server_thread ()
diego1332 11:88cba231c882 68 {
mbedAustin 7:a5ead1402704 69 printf("\nServer IP Address is %s\n", eth.getIPAddress());
diego1332 11:88cba231c882 70
emilmont 1:5cebe0e38cd2 71 TCPSocketServer server;
emilmont 3:36fd3cfad85a 72 server.bind(ECHO_SERVER_PORT);
emilmont 3:36fd3cfad85a 73 server.listen();
diego1332 11:88cba231c882 74
emilmont 1:5cebe0e38cd2 75 while (true) {
emilmont 1:5cebe0e38cd2 76 printf("\nWait for new connection...\n");
emilmont 1:5cebe0e38cd2 77 TCPSocketConnection client;
emilmont 1:5cebe0e38cd2 78 server.accept(client);
emilmont 3:36fd3cfad85a 79 client.set_blocking(false, 1500); // Timeout after (1.5)s
diego1332 11:88cba231c882 80
diego1332 11:88cba231c882 81 conectado = true;
pauloludewig 9:eb22e90a3c03 82 printf("\nConnection from: %s\n", client.get_address());
emilmont 1:5cebe0e38cd2 83 char buffer[256];
emilmont 1:5cebe0e38cd2 84 while (true) {
emilmont 3:36fd3cfad85a 85 int n = client.receive(buffer, sizeof(buffer));
emilmont 1:5cebe0e38cd2 86 if (n <= 0) break;
diego1332 11:88cba231c882 87
mbedAustin 7:a5ead1402704 88 // print received message to terminal
mbedAustin 7:a5ead1402704 89 buffer[n] = '\0';
mbedAustin 7:a5ead1402704 90 printf("Received message from Client :'%s'\n",buffer);
diego1332 11:88cba231c882 91
diego1332 11:88cba231c882 92 int pos = buffer[8] - '0';
diego1332 11:88cba231c882 93
diego1332 11:88cba231c882 94 if(pos == 1)setpoint = 20;
diego1332 11:88cba231c882 95 else if(pos == 2)setpoint = 35;
diego1332 11:88cba231c882 96 else if(pos == 3)setpoint = 50;
diego1332 11:88cba231c882 97 printf("Setpoint: %d\n", setpoint);
diego1332 11:88cba231c882 98
diego1332 11:88cba231c882 99 m_ref.lock();
diego1332 11:88cba231c882 100 // ref = atof(buffer);
diego1332 11:88cba231c882 101 sprintf (buffer, "%s", client.get_address());
diego1332 11:88cba231c882 102 m_ref.unlock();
diego1332 11:88cba231c882 103
pauloludewig 9:eb22e90a3c03 104 m_ref.lock();
diego1332 10:2eaab13dec3c 105 sprintf (buffer, "%s", client.get_address());
pauloludewig 9:eb22e90a3c03 106 m_ref.unlock();
diego1332 11:88cba231c882 107
pauloludewig 9:eb22e90a3c03 108 // print sending message to terminal
mbedAustin 7:a5ead1402704 109 printf("Sending message to Client: '%s'\n",buffer);
diego1332 11:88cba231c882 110
mbedAustin 7:a5ead1402704 111 // Echo received message back to client
diego1332 11:88cba231c882 112 //client.send_all(buffer, n);
diego1332 11:88cba231c882 113 // if (n <= 0) break;
diego1332 11:88cba231c882 114 memset(buffer,0,256);
emilmont 1:5cebe0e38cd2 115 }
diego1332 11:88cba231c882 116
emilmont 1:5cebe0e38cd2 117 client.close();
emilmont 1:5cebe0e38cd2 118 }
emilmont 1:5cebe0e38cd2 119 }
diego1332 11:88cba231c882 120
diego1332 11:88cba231c882 121 void udp_broadcast ()
diego1332 11:88cba231c882 122 {
diego1332 10:2eaab13dec3c 123 UDPSocket sock;
diego1332 10:2eaab13dec3c 124 sock.init();
diego1332 10:2eaab13dec3c 125 sock.set_broadcasting();
diego1332 11:88cba231c882 126
diego1332 10:2eaab13dec3c 127 Endpoint broadcast;
diego1332 10:2eaab13dec3c 128 broadcast.set_address("255.255.255.255", BROADCAST_PORT);
diego1332 11:88cba231c882 129
diego1332 10:2eaab13dec3c 130 char out_buffer[] = "diego";
diego1332 11:88cba231c882 131
diego1332 10:2eaab13dec3c 132 while (!conectado) {
diego1332 10:2eaab13dec3c 133 printf("Broadcasting...\n");
diego1332 10:2eaab13dec3c 134 sock.sendTo(broadcast, out_buffer, sizeof(out_buffer));
pauloludewig 9:eb22e90a3c03 135 Thread::wait(1000);
pauloludewig 9:eb22e90a3c03 136 }
diego1332 11:88cba231c882 137 printf("Broadcast finalizado!\n");
pauloludewig 9:eb22e90a3c03 138 }
diego1332 11:88cba231c882 139
diego1332 11:88cba231c882 140 void leitura_thread ()
diego1332 11:88cba231c882 141 {
diego1332 11:88cba231c882 142
pauloludewig 9:eb22e90a3c03 143 while (true) {
diego1332 10:2eaab13dec3c 144 m_dist.lock();
diego1332 10:2eaab13dec3c 145 dist = usensor.getCm();
diego1332 10:2eaab13dec3c 146 dist = dist/RESOL;
diego1332 10:2eaab13dec3c 147 dist = dist*RESOL;
diego1332 11:88cba231c882 148 // printf("cm:%ld\n",dist );
diego1332 10:2eaab13dec3c 149 if(dist>DIST_MAX) dist = DIST_MAX;
diego1332 10:2eaab13dec3c 150 if(dist<DIST_MIN) dist = DIST_MIN;
diego1332 10:2eaab13dec3c 151 m_dist.unlock();
diego1332 11:88cba231c882 152
diego1332 10:2eaab13dec3c 153 Thread::wait(2);
pauloludewig 9:eb22e90a3c03 154 }
pauloludewig 9:eb22e90a3c03 155 }
pauloludewig 9:eb22e90a3c03 156
diego1332 11:88cba231c882 157 void controle_thread()
diego1332 11:88cba231c882 158 {
diego1332 11:88cba231c882 159
diego1332 11:88cba231c882 160 float yant = 0;
diego1332 11:88cba231c882 161 float xant = 0;
diego1332 11:88cba231c882 162 float y = 0;
diego1332 11:88cba231c882 163 float x = 0;
diego1332 11:88cba231c882 164 float k = 60;
diego1332 11:88cba231c882 165 float erro = 0;
diego1332 11:88cba231c882 166 float erroant = 0;
diego1332 11:88cba231c882 167 int motor;
diego1332 11:88cba231c882 168
diego1332 11:88cba231c882 169 while(true) {
diego1332 11:88cba231c882 170
diego1332 11:88cba231c882 171 m_dist.lock();
diego1332 11:88cba231c882 172 x = dist;
diego1332 10:2eaab13dec3c 173
diego1332 11:88cba231c882 174 erro = setpoint - x;
diego1332 11:88cba231c882 175
diego1332 11:88cba231c882 176 m_dist.unlock();
diego1332 11:88cba231c882 177
diego1332 11:88cba231c882 178 y = 0.1*yant + erro*k - erroant*k*0.90;
diego1332 11:88cba231c882 179
diego1332 11:88cba231c882 180 motor = y + POS_CENTRO;
diego1332 10:2eaab13dec3c 181
diego1332 11:88cba231c882 182 //printf("y: %.01f ",y );
diego1332 11:88cba231c882 183 //printf("erro: %.01f ",erro );
diego1332 11:88cba231c882 184 erroant = erro;
diego1332 11:88cba231c882 185 xant = x;
diego1332 11:88cba231c882 186 yant = y;
diego1332 11:88cba231c882 187
diego1332 11:88cba231c882 188 if(motor>POS_MAX) motor = POS_MAX;
diego1332 11:88cba231c882 189 if(motor<POS_MIN) motor = POS_MIN;
diego1332 11:88cba231c882 190 Servo1.SetPosition(motor);
diego1332 11:88cba231c882 191 //printf("motor: %d\n",motor );
diego1332 11:88cba231c882 192
diego1332 11:88cba231c882 193
diego1332 11:88cba231c882 194
diego1332 11:88cba231c882 195 Thread::wait(PERIODO);
pauloludewig 9:eb22e90a3c03 196 }
diego1332 11:88cba231c882 197
diego1332 11:88cba231c882 198 }