Diego Carrillo / Mbed 2 deprecated ProjetoSO

aaaaa

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

Fork of TCPEchoServer by Paulo Ludewig

main.cpp@10:2eaab13dec3c, 2017-12-13 (annotated)

Committer:
diego1332
Date:
Wed Dec 13 21:04:34 2017 +0000
Revision:
10:2eaab13dec3c
Parent:
9:eb22e90a3c03
Child:
11:88cba231c882
teste;

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