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();
}
}
}
void uartToUdp()
{
bool sendIt = false;
if (Serial.available() > 0)
{
uartBuffer[uartCounter] = Serial.read();
if (uartBuffer[uartCounter] == HDLC_SS_BYTE)
{
if (hdlcStart)sendIt = true;
else
{
hdlcStart = true;
Timer1.start();
}
}
else if (hdlcStart)
{
++uartCounter;
}
}
if (sendIt)
{
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write(uartBuffer, uartCounter);
Udp.endPacket();
resetUartBuffer();
}
}
// 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 Notification::sendUDPNotification(EthernetUDP &udpSocket, aJsonObject *pushurl_channel, char *payload1, extData payload2)
{
Serial.print(F("Sending a UDP response: "));
Serial.println(payload1);
udpSocket.beginPacket(udpSocket.remoteIP(), udpSocket.remotePort());
// The actual payload
udpSocket.write(payload1);
// Extra payload is generated by running a callback function
if (payload2 != NULL)
{
(*payload2)(&udpSocket);
}
udpSocket.write("}"); // The end of the JSON data, i.e. '}'
udpSocket.endPacket();
}
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();
}
}
}
SNMP_API_STAT_CODES AgentuinoClass::responsePdu(SNMP_PDU *pdu)
{
int32_u u;
byte i;
//
// Length of entire SNMP packet
_packetPos = 0; // 23
_packetSize = 25 + sizeof(pdu->requestId) + sizeof(pdu->error) + sizeof(pdu->errorIndex) + pdu->OID.size + pdu->VALUE.size;
//
memset(_packet, 0, SNMP_MAX_PACKET_LEN);
//
if ( _dstType == SNMP_PDU_SET ) {
_packetSize += _setSize;
} else {
_packetSize += _getSize;
}
//
_packet[_packetPos++] = (byte)SNMP_SYNTAX_SEQUENCE; // type
_packet[_packetPos++] = (byte)_packetSize - 2; // length
//
// SNMP version
_packet[_packetPos++] = (byte)SNMP_SYNTAX_INT; // type
_packet[_packetPos++] = 0x01; // length
_packet[_packetPos++] = 0x00; // value
//
// SNMP community string
_packet[_packetPos++] = (byte)SNMP_SYNTAX_OCTETS; // type
if ( _dstType == SNMP_PDU_SET ) {
_packet[_packetPos++] = (byte)_setSize; // length
for ( i = 0; i < _setSize; i++ ) {
_packet[_packetPos++] = (byte)_setCommName[i];
}
} else {
_packet[_packetPos++] = (byte)_getSize; // length
for ( i = 0; i < _getSize; i++ ) {
_packet[_packetPos++] = (byte)_getCommName[i];
}
}
//
// SNMP PDU
_packet[_packetPos++] = (byte)pdu->type;
_packet[_packetPos++] = (byte)( sizeof(pdu->requestId) + sizeof((int32_t)pdu->error) + sizeof(pdu->errorIndex) + pdu->OID.size + pdu->VALUE.size + 14 );
//
// Request ID (size always 4 e.g. 4-byte int)
_packet[_packetPos++] = (byte)SNMP_SYNTAX_INT; // type
_packet[_packetPos++] = (byte)sizeof(pdu->requestId);
u.int32 = pdu->requestId;
_packet[_packetPos++] = u.data[3];
_packet[_packetPos++] = u.data[2];
_packet[_packetPos++] = u.data[1];
_packet[_packetPos++] = u.data[0];
//
// Error (size always 4 e.g. 4-byte int)
_packet[_packetPos++] = (byte)SNMP_SYNTAX_INT; // type
_packet[_packetPos++] = (byte)sizeof((int32_t)pdu->error);
u.int32 = pdu->error;
_packet[_packetPos++] = u.data[3];
_packet[_packetPos++] = u.data[2];
_packet[_packetPos++] = u.data[1];
_packet[_packetPos++] = u.data[0];
//
// Error Index (size always 4 e.g. 4-byte int)
_packet[_packetPos++] = (byte)SNMP_SYNTAX_INT; // type
_packet[_packetPos++] = (byte)sizeof(pdu->errorIndex);
u.int32 = pdu->errorIndex;
_packet[_packetPos++] = u.data[3];
_packet[_packetPos++] = u.data[2];
_packet[_packetPos++] = u.data[1];
_packet[_packetPos++] = u.data[0];
//
// Varbind List
_packet[_packetPos++] = (byte)SNMP_SYNTAX_SEQUENCE; // type
_packet[_packetPos++] = (byte)( pdu->OID.size + pdu->VALUE.size + 6 ); //4
//
// Varbind
_packet[_packetPos++] = (byte)SNMP_SYNTAX_SEQUENCE; // type
_packet[_packetPos++] = (byte)( pdu->OID.size + pdu->VALUE.size + 4 ); //2
//
// ObjectIdentifier
_packet[_packetPos++] = (byte)SNMP_SYNTAX_OID; // type
_packet[_packetPos++] = (byte)(pdu->OID.size);
for ( i = 0; i < pdu->OID.size; i++ ) {
_packet[_packetPos++] = pdu->OID.data[i];
}
//
// Value
_packet[_packetPos++] = (byte)pdu->VALUE.syntax; // type
_packet[_packetPos++] = (byte)(pdu->VALUE.size);
for ( i = 0; i < pdu->VALUE.size; i++ ) {
_packet[_packetPos++] = pdu->VALUE.data[i];
}
//
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
Udp.write(_packet, _packetSize);
Udp.endPacket();
// Udp.write(_packet, _packetSize, _dstIp, _dstPort);
//
return SNMP_API_STAT_SUCCESS;
}
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);
}
uint16_t SNMPClass::remotePort(){
return Udp.remotePort();
}