time_t getNtpTime()
{
while (Udp.parsePacket() > 0) ; // discard any previously received packets
Serial.println("Transmit NTP Request");
sendNTPpacket(ntp_server);
uint32_t beginWait = millis();
while (millis() - beginWait < 2000) {
int size = Udp.parsePacket();
if (size >= NTP_PACKET_SIZE) {
Serial.println("Receive NTP Response");
Udp.read(packetBuffer, NTP_PACKET_SIZE); // read packet into the buffer
unsigned long secsSince1900;
unsigned long fracSecs;
// convert four bytes starting at location 40 to a long integer
secsSince1900 = (unsigned long)packetBuffer[40] << 24;
secsSince1900 |= (unsigned long)packetBuffer[41] << 16;
secsSince1900 |= (unsigned long)packetBuffer[42] << 8;
secsSince1900 |= (unsigned long)packetBuffer[43];
fracSecs = (unsigned long)packetBuffer[44] << 24;
fracSecs |= (unsigned long)packetBuffer[45] << 16;
fracSecs |= (unsigned long)packetBuffer[46] << 8;
fracSecs |= (unsigned long)packetBuffer[47];
return ((time_t)(secsSince1900 - 2208988800UL)<<32) + fracSecs;
}
}
Serial.println("No NTP Response :-(");
return 0; // return 0 if unable to get the time
}
/*
* Perform housekeeping needed to keep ntp time up to date.
*
* This essentially involves occasionally re-syncing our
* time with a time server so our internal clock doesnt
* drift too far.
*
*/
void serviceNtpTime() {
switch (ntpState) {
case NTP_IN_SYNC :
// Check to see if we need to re-sync
if ((ntpLastSyncTime + SYNC_INTERVAL) < now()) {
sendTimeRequest();
}
break;
case WAITING_FOR_RESPONSE :
// See if we've received a response to a sync request yet
if ( Udp.parsePacket() ) {
ntpLastSyncTime = parseNtpPacket();
setTime(ntpLastSyncTime);
ntpState = NTP_IN_SYNC;
Serial.print("Updated time: ");
Serial.println(now());
} else if ((lastNtpRequestTime + MAX_WAIT_INTERVAL) < now()) {
// We've been waiting a while now and we're probably not going
// to get a response, so let's try the next server in the list
Serial.println("No ntp response");
sendTimeRequest();
}
break;
}
}
int readUdp( char * buf, int size )
{
if ( ! active ) return 0 ;
int count = Udp.parsePacket() ;
if ( count ) Udp.read( buf, size ) ;
return count ;
}
void AgentuinoClass::listen(void) {
// if bytes are available in receive buffer
// and pointer to a function (delegate function)
// isn't null, trigger the function
Udp.parsePacket();
if (Udp.available() && _callback != NULL) (*_callback)();
}
void loop() {
// if there's data available, read a packet
int packetSize = Udp.parsePacket();
char Size[4];
if(packetSize)
{
if(packetSize == 4)
{
IPAddress remote = Udp.remoteIP();
// read the packet into packetBufffer
Udp.read(packetBuffer,UDP_TX_PACKET_MAX_SIZE);
char Command[4];
for(int nCom = 0; nCom < 4; nCom++)
{
Command[nCom] = packetBuffer[nCom];
}
if (Command[0] == 't')
{
digitalWrite(13, HIGH);
}
else if (Command[0] == 'r')
{
digitalWrite(12, HIGH);
}
else if (Command[0] == 'e')
{
digitalWrite(10, HIGH);
}
else if (Command[0] == 'w')
{
analogWrite(11, 65);
}
else
{
digitalWrite(13, LOW);
digitalWrite(12, LOW);
analogWrite(11, 0);
}
// send a reply, to the IP address and port that sent us the packet we received
itoa(packetSize, Size, 10);
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write(ReplyBuffer);
Udp.write(packetBuffer);
Udp.write(Size);
Udp.endPacket();
delay(1000);
}
else
{
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write("NOPE");
Udp.endPacket();
}
}
}
// This function's purpose is to receive data and prepare it for parsing
void RobotOpenClass::handleData() {
_packetBufferSize = Udp.parsePacket();
// If there's data, read the packet in
if (_packetBufferSize > 0) {
_remotePort = Udp.remotePort();
_remoteIp = Udp.remoteIP();
Udp.read(_packetBufferAccessor, 256);
parsePacket(); // Data is all set, time to parse through it
}
}
void udpToUart()
{
int packetSize = Udp.parsePacket();
if(packetSize)
{
// read the packet into packetBufffer
Udp.read(udpBuffer,MAX_HDLC_FRAME_SIZE);
Serial.write(HDLC_SS_BYTE);
Serial.write(udpBuffer, packetSize);
Serial.write(0x12);
Serial.write(0x34);
Serial.write(HDLC_SS_BYTE);
}
}
roveEthernet_Error roveEthernet_GetUdpMsg(roveIP* senderIP, void* buffer, size_t bufferSize)
{
int packetSize = udpReceiver.parsePacket();
if (packetSize > 0) //if there is a packet available
{
udpReceiver.read((char*)buffer, bufferSize);
*senderIP = udpReceiver.remoteIP();
return ROVE_ETHERNET_ERROR_SUCCESS;
}
else
{
return ROVE_ETHERNET_ERROR_WOULD_BLOCK;
}
}
void UdpController::Loop()
{
unsigned long currentMillis = millis();
int packetSize = Udp.parsePacket();
if (packetSize && packetSize == 3)
{
Udp.read(Buffer, UDP_TX_PACKET_MAX_SIZE);
if (Buffer[0] == 0x28 && Buffer[1] == 0x06)
{
_traffic->SetMode((LightMode)Buffer[2]);
_lastUpdateMillis = currentMillis;
}
}
if ((currentMillis - _lastUpdateMillis) > UpdateIntervalMillis)
{
_traffic->ShowInconclusive();
}
}
boolean SNMPClass::listen(void)
{
// if bytes are available in receive buffer
// and pointer to a function (delegate function)
// isn't null, trigger the function
if(Udp.parsePacket() > 1024){
_udp_extra_data_packet = true;
}else{
_udp_extra_data_packet = false;
}
if (Udp.available()){
if(_callback != NULL){
(*_callback)();
}else{
return true;
}
}
return false;
}
unsigned long getNtpTime()
{
sendNTPpacket(timeServer); // send an NTP packet to a time server
// wait to see if a reply is available
delay(1000);
if ( Udp.parsePacket() ) {
// We've received a packet, read the data from it
Udp.read(packetBuffer,NTP_PACKET_SIZE); // read the packet into the buffer
//the timestamp starts at byte 40 of the received packet and is four bytes,
// or two words, long. First, esxtract the two words:
unsigned long highWord = word(packetBuffer[40], packetBuffer[41]);
unsigned long lowWord = word(packetBuffer[42], packetBuffer[43]);
// combine the four bytes (two words) into a long integer
// this is NTP time (seconds since Jan 1 1900):
unsigned long secsSince1900 = highWord << 16 | lowWord;
#ifdef LOGGING
Serial.print("Seconds since Jan 1 1900 = " );
Serial.println(secsSince1900);
#endif
// now convert NTP time into everyday time:
// Unix time starts on Jan 1 1970. In seconds, that's 2208988800:
const unsigned long seventyYears = 2208988800UL;
// subtract seventy years:
unsigned long epoch = secsSince1900 - seventyYears;
// print Unix time:
#ifdef LOGGING
Serial.print(UNIX_TIME);
Serial.println(epoch);
#endif
return epoch;
}
return 0; // return 0 if unable to get the time
}
void commandInterfaceTick() {
int packetSize = cmdsock.parsePacket();
if(cmdsock.available()) {
// read the packet into packetBufffer
cmdsock.read(udpPacketBuffer, PACKET_SIZE);
if(memcmp("INGV\0", udpPacketBuffer, 5) != 0) {
return;
}
bool reboot = false;
unsigned long unixTimeM = getUNIXTime();
unsigned long uptime = getUNIXTime() - getBootTime();
byte macaddress[6] = { 0 };
getMACAddress(macaddress);
uint32_t probeSpeed = getProbeSpeedStatistic();
uint32_t freeramkb = freeMemory();
float latency = 0;
if(udpPacketBuffer[5] == PKTTYPE_GETINFO) {
latency = tcpLatency();
}
float longitude = 0;
float latitude = 0;
switch(udpPacketBuffer[5]) {
case PKTTYPE_DISCOVERY:
// Reply to discovery
udpPacketBuffer[5] = PKTTYPE_DISCOVERY_REPLY;
memcpy(udpPacketBuffer + 6, macaddress, 6);
memcpy(udpPacketBuffer + 12, getVersionAsString().c_str(), 4);
memcpy(udpPacketBuffer + 16, "uno", 3);
break;
case PKYTYPE_PING:
// Reply to ping
udpPacketBuffer[5] = PKYTYPE_PONG;
break;
case PKTTYPE_SENDGPS:
// Get coords
udpPacketBuffer[5] = PKTTYPE_OK;
memcpy(&latitude, udpPacketBuffer + 12, 4);
memcpy(&longitude, udpPacketBuffer + 16, 4);
reverse4bytes((byte*)&latitude);
reverse4bytes((byte*)&longitude);
break;
case PKTTYPE_REBOOT:
// Reboot
// Reply with OK
udpPacketBuffer[5] = PKTTYPE_OK;
reboot = true;
break;
case PKTTYPE_GETINFO:
udpPacketBuffer[5] = PKTTYPE_GETINFO_REPLY;
memcpy(udpPacketBuffer + 6, macaddress, 6);
memcpy(udpPacketBuffer + 28, &uptime, 4);
memcpy(udpPacketBuffer + 32, &unixTimeM, 4);
memcpy(udpPacketBuffer + 36, VERSION, 4);
memcpy(udpPacketBuffer + 40, &freeramkb, 4);
memcpy(udpPacketBuffer + 44, &latency, 4);
memcpy(udpPacketBuffer + 53, "uno", 3);
memcpy(udpPacketBuffer + 57, "MMA7361", 7);
memcpy(udpPacketBuffer + 65, &probeSpeed, 4);
break;
#ifdef RESET_ENABLED
case PKTTYPE_RESET:
initEEPROM();
reboot = true;
break;
#endif
default:
// Unknown packet or invalid command
return;
}
if(longitude != 0 && latitude != 0) {
setLongitude(longitude);
setLatitude(latitude);
}
cmdsock.beginPacket(cmdsock.remoteIP(), cmdsock.remotePort());
cmdsock.write(udpPacketBuffer, PACKET_SIZE);
cmdsock.endPacket();
cmdsock.flush();
if(reboot) {
soft_restart();
}
}
}
bool gatewayTransportSend(MyMessage &message)
{
bool ret = true;
char *_ethernetMsg = protocolFormat(message);
setIndication(INDICATION_GW_TX);
_w5100_spi_en(true);
#if defined(MY_CONTROLLER_IP_ADDRESS)
#if defined(MY_USE_UDP)
_ethernetServer.beginPacket(_ethernetControllerIP, MY_PORT);
_ethernetServer.write(_ethernetMsg, strlen(_ethernetMsg));
// returns 1 if the packet was sent successfully
ret = _ethernetServer.endPacket();
#else
EthernetClient client;
#if defined(MY_CONTROLLER_URL_ADDRESS)
if (client.connected() || client.connect(MY_CONTROLLER_URL_ADDRESS, MY_PORT)) {
#else
if (client.connected() || client.connect(_ethernetControllerIP, MY_PORT)) {
#endif
client.write(_ethernetMsg, strlen(_ethernetMsg));
}
else {
// connecting to the server failed!
ret = false;
}
#endif
#else
// Send message to connected clients
#if defined(MY_GATEWAY_ESP8266)
for (uint8_t i = 0; i < ARRAY_SIZE(clients); i++)
{
if (clients[i] && clients[i].connected())
{
clients[i].write((uint8_t*)_ethernetMsg, strlen(_ethernetMsg));
}
}
#else
_ethernetServer.write(_ethernetMsg);
#endif
#endif
_w5100_spi_en(false);
return ret;
}
#if defined(MY_GATEWAY_ESP8266)
bool _readFromClient(uint8_t i) {
while (clients[i].connected() && clients[i].available()) {
char inChar = clients[i].read();
if (inputString[i].idx < MY_GATEWAY_MAX_RECEIVE_LENGTH - 1) {
// if newline then command is complete
if (inChar == '\n' || inChar == '\r') {
// Add string terminator and prepare for the next message
inputString[i].string[inputString[i].idx] = 0;
debug(PSTR("Client %d: %s\n"), i, inputString[i].string);
inputString[i].idx = 0;
if (protocolParse(_ethernetMsg, inputString[i].string)) {
return true;
}
} else {
// add it to the inputString:
inputString[i].string[inputString[i].idx++] = inChar;
}
} else {
// Incoming message too long. Throw away
debug(PSTR("Client %d: Message too long\n"), i);
inputString[i].idx = 0;
// Finished with this client's message. Next loop() we'll see if there's more to read.
break;
}
}
return false;
}
#else
bool _readFromClient() {
while (client.connected() && client.available()) {
char inChar = client.read();
if (inputString.idx < MY_GATEWAY_MAX_RECEIVE_LENGTH - 1) {
// if newline then command is complete
if (inChar == '\n' || inChar == '\r') {
// Add string terminator and prepare for the next message
inputString.string[inputString.idx] = 0;
debug(PSTR("Eth: %s\n"), inputString.string);
inputString.idx = 0;
if (protocolParse(_ethernetMsg, inputString.string)) {
return true;
}
} else {
// add it to the inputString:
inputString.string[inputString.idx++] = inChar;
}
} else {
// Incoming message too long. Throw away
debug(PSTR("Eth: Message too long\n"));
inputString.idx = 0;
// Finished with this client's message. Next loop() we'll see if there's more to read.
break;
}
}
return false;
}
#endif
bool gatewayTransportAvailable()
{
_w5100_spi_en(true);
#if !defined(MY_IP_ADDRESS) && defined(MY_GATEWAY_W5100)
// renew IP address using DHCP
gatewayTransportRenewIP();
#endif
#ifdef MY_USE_UDP
int packet_size = _ethernetServer.parsePacket();
if (packet_size) {
//debug(PSTR("UDP packet available. Size:%d\n"), packet_size);
setIndication(INDICATION_GW_RX);
#if defined(MY_GATEWAY_ESP8266)
_ethernetServer.read(inputString[0].string, MY_GATEWAY_MAX_RECEIVE_LENGTH);
inputString[0].string[packet_size] = 0;
debug(PSTR("UDP packet received: %s\n"), inputString[0].string);
return protocolParse(_ethernetMsg, inputString[0].string);
#else
_ethernetServer.read(inputString.string, MY_GATEWAY_MAX_RECEIVE_LENGTH);
inputString.string[packet_size] = 0;
debug(PSTR("UDP packet received: %s\n"), inputString.string);
_w5100_spi_en(false);
return protocolParse(_ethernetMsg, inputString.string);
#endif
}
#else
#if defined(MY_GATEWAY_ESP8266)
// ESP8266: Go over list of clients and stop any that are no longer connected.
// If the server has a new client connection it will be assigned to a free slot.
bool allSlotsOccupied = true;
for (uint8_t i = 0; i < ARRAY_SIZE(clients); i++) {
if (!clients[i].connected()) {
if (clientsConnected[i]) {
debug(PSTR("Client %d disconnected\n"), i);
clients[i].stop();
}
//check if there are any new clients
if (_ethernetServer.hasClient()) {
clients[i] = _ethernetServer.available();
inputString[i].idx = 0;
debug(PSTR("Client %d connected\n"), i);
gatewayTransportSend(buildGw(_msg, I_GATEWAY_READY).set("Gateway startup complete."));
if (presentation)
presentation();
}
}
bool connected = clients[i].connected();
clientsConnected[i] = connected;
allSlotsOccupied &= connected;
}
if (allSlotsOccupied && _ethernetServer.hasClient()) {
//no free/disconnected spot so reject
debug(PSTR("No free slot available\n"));
EthernetClient c = _ethernetServer.available();
c.stop();
}
// Loop over clients connect and read available data
for (uint8_t i = 0; i < ARRAY_SIZE(clients); i++) {
if (_readFromClient(i)) {
setIndication(INDICATION_GW_RX);
_w5100_spi_en(false);
return true;
}
}
#else
// W5100/ENC module does not have hasClient-method. We can only serve one client at the time.
EthernetClient newclient = _ethernetServer.available();
// if a new client connects make sure to dispose any previous existing sockets
if (newclient) {
if (client != newclient) {
client.stop();
client = newclient;
debug(PSTR("Eth: connect\n"));
_w5100_spi_en(false);
gatewayTransportSend(buildGw(_msg, I_GATEWAY_READY).set("Gateway startup complete."));
_w5100_spi_en(true);
if (presentation)
presentation();
}
}
if (client) {
if (!client.connected()) {
debug(PSTR("Eth: disconnect\n"));
client.stop();
} else {
if (_readFromClient()) {
setIndication(INDICATION_GW_RX);
_w5100_spi_en(false);
return true;
}
}
}
#endif
#endif
_w5100_spi_en(false);
return false;
}
void loop() {
if ((unsigned long) (millis() - previousEth) >= interval_ethernet) { //entra a cada 50ms
previousEth = millis();
if(count_eth == 0) { //espera completar um ciclo de todas as funções chamadas
int packetSize = Udp.parsePacket();
count_eth = 2;
Serial.println("entrou eth");
received = 0;
if (packetSize) {
set_debug(PC3);
#ifdef debug
Serial.println("entrou eth RX ");
Serial.print("Received packet of size ");
Serial.println(packetSize);
Serial.print("From ");
IPAddress remote = Udp.remoteIP();
for (int i =0; i < 4; i++)
{
Serial.print(remote[i], DEC);
if (i < 3)
{
Serial.print(".");
}
}
Serial.print(", port ");
Serial.println(Udp.remotePort());
Serial.println("Contents:");
Serial.println(packetBuffer);
#endif
received = Udp.read();
charBuffer.put(received);
#ifdef debug
Serial.println("\rchar recebido:");
Serial.write(received);
#endif
/////////////////////////////
clear_debug(PC3);
}
} else {
count_eth--;
Serial.println("entrou eth NADA");
if (charBuffer.getSize() > 5) charBuffer.clear();
if( charBuffer.getSize() > 0 )
received = charBuffer.get();
Serial.println("received:");
Serial.println(received);
}
}
#ifdef debug
Serial.println("client disconnected");
#endif
/****************FIM ROTINA ETHERNET ***************************************************/
if ((unsigned long) (millis() - previousADC) >= interval_adc) { //entra a cada 60ms
previousADC = millis();
set_debug(PC4);
if (count_adc == 0) {
count_adc = 1;
/*testa obstaculo IR*/
Serial.println("entrou adc OBSTACULO ");
IR_obstaculo = 0;
IR_obstaculo = verificaObstaculoIR();
#ifdef debug
Serial.println("distancia_ir");
Serial.println(IR_obstaculo);
Serial.println("\r");
#endif
if (IR_obstaculo > IR_OBSTACLE_THRESHOLD)
obstacle_flag = 0;
else if (IR_obstaculo > IR_OBSTACLE_THRESHOLD
&& IR_obstaculo < IR_OBSTACLE_UPPER_THRESHOLD)
obstacle_flag = 1;
else if (IR_obstaculo <= IR_OBSTACLE_THRESHOLD)
obstacle_flag = 2;
clear_debug(PC4);
} else {
count_adc=0;
Serial.println("entrou adc NADA ");
}
}
/***************** FIM ROTINA ADC********************************/
if ((unsigned long) (millis() - previousMotores) >= interval_motores) { //entra a cada 100ms
previousMotores = millis();
if (ciclosClock_motor == 2) { //duas bordas de subida depois de acionar o motor (200ms depois de acionar o motor)
Serial.println("motor stop ");
stopMove();
ciclosClock_motor = 0;
}
if (count_motor == 0) {
Serial.println("motor ANDA ");
if (obstacle_flag == 0 || obstacle_flag == 1) {
count_motor = 1;
/*move motores*/
switch (received) {
case 'u':
goForward(obstacle_flag);
break;
case 'd':
goBack(obstacle_flag);
break;
case 'l':
goLeft(obstacle_flag);
break;
case 'r':
goRight(obstacle_flag);
break;
default:
received = 0;
stopMove();
break;
}
} else {
if (received == 'd') {
goBack(WITH_CARE);
} else {
stopMove();
}
}
} else {
ciclosClock_motor++;
count_motor = 0;
Serial.println("ciclosClock_motor++ ");
}
}
// _delay_ms(500);
}
