![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
aaaaa
Dependencies: EthernetInterface HC-SR04 PID Servo mbed-rtos mbed
Fork of TCPEchoServer by
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?
User | Revision | Line number | New 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 [] |