IO interface with Arduino over Ethernet

/*
https://forum.linuxcnc.org/media/kunena/attachments/9308/udpcomms.txt

Modified 	2-13-2017 - begin, Erich Stein
			2-14-2017 - process code, make adjustments
			2-20-2017 - debugged transmit failure, continue to speed test

The purpose of this program is to provide a wireless hardware interface to LinuxCNC.

Features:

	Input:

	1 Encoder interface					2 DI			- Multipurpose jog positioning
	1 Analog potentiometer input		1 AI			- Speed feed override
	2 Pushbuttons						2 DI			- 1) Toggle encoder axis, 2) Toggle encoder precision
	3 Switches							3 DI			- Machine on-ready, 2 undefined
	
	Output:
	2 Digital signals output				2 DO		- Machine on, Error stop
	3 Digital signals output 				3 DO		- System homed x, y, z
	Sound buzzer - TODO Optional 			1 DO		- Audio output interface, errors, ready, done, input required
	
	Budget:								6 + 6			- 12 SIGNALS, speaker, audio amplifier
	

*/

//#include <SPI.h>         											// needed for Arduino versions later than 0018
//#include <Ethernet.h>
//#include <EthernetUdp.h>         									// UDP library from: bjoern@cs.stanford.edu 12/30/2008
//#include <string.h>
//#include <stdio.h>
//#include <stdlib.h>
//#include <ctype.h>

#include <WiFiEsp.h>
#include <WiFiEspUdp.h>
#include <Encoder.h>

// emulate Serial1 if not present
#ifndef HAVE_HWSERIAL1
  #include "SoftwareSerial.h"
  SoftwareSerial Serial1(5, 4); // RX, TX
#endif

//#define cDebug true
#define cDebug false

WiFiEspUDP Udp;														// An EthernetUDP instance to let us send and receive packets over UDP
//IPAddress ipLocal(192, 168, 16, 200);								// local ip address
IPAddress ipRemote(192, 168, 16, 9);								// remote ip address
char ssid[]		= "my_local_wifi_ssid";
char pass[]		= "mySSIDpassword";

unsigned int portLocal = 8888;      								// local port 
unsigned int portRemote = 8888;										// remote port

#define iPacketSize 100
byte rxChar;
char workBuffer[iPacketSize];
char rxTokens[5];
char *newToken;
const char sComma[2] = ",";
char *token;

//char packetBuffer[UDP_TX_PACKET_MAX_SIZE]; 							// buffer to hold incoming packet,
char packetBuffer[iPacketSize]; 										// buffer to hold incoming packet,
char  ReplyBuffer[iPacketSize];// = "acknowledged";       				// a string to send back
String sMessage = "";
uint16_t adcResult;

int pinADC=A0;															// pin number for analog input
uint8_t firstbyte=0x0;

const byte byPinsInput = 6;												// number of input pins.
const byte  byPinsOutput = 6;											// number of output pins.  For some reason, is receiving ?!?!
const byte byPinsOutputAnalog = 2;										// number of analog input pins
byte pinMapOut[byPinsOutput] = {A1,A2,8,9,10,11};						// Output pins
byte pinMapIn[byPinsInput] = {A1,A2,8,9,10,11};							// Output pins
uint8_t dacPinMap[1] = {A3};											// appears to be only 1 (2?) analog outputs

// encoder section
#define ENCODER_OPTIMIZE_INTERRUPTS
//Encoder myEncoder(5, 4);												// Encoder interface
Encoder myEncoder(3, 2);												// Encoder interface
char cCounter[2] = {0,0};
char oldPosition = -1;
char newPosition = 0;

void setup() 
{
	pinMode(2, INPUT_PULLUP);
	pinMode(3, INPUT_PULLUP);
	//pinMode(4, INPUT_PULLUP);
	//pinMode(5, INPUT_PULLUP);

	Serial.begin(115200);
	Serial.println(F("Initiate program emc2interface v1.1.1"));

	// start the Ethernet and UDP:
	Serial1.begin(9600);
	WiFi.init(&Serial1);
	WiFi.begin(ssid, pass);
	
	while (WiFi.status() != WL_CONNECTED)
	{
		delay(64);
		Serial.print(F(". "));
	}

	Serial.println();
	Serial.print(F("Connect WiFi, IP: "));  
	Serial.println(WiFi.localIP());

	Udp.begin(portLocal);
	
}

void loop() 
{
	//	encodeTx();															// moved from [here]

	// if there's data available, read a packet
	int packetSize = Udp.parsePacket();
	if(packetSize)
	{
		
		Udp.read(packetBuffer, iPacketSize);
		
		if (cDebug)
		{
			Serial.print(F("Packet size:"));
			Serial.print(packetSize);
			Serial.print(F(" Buffer:"));
			Serial.println(packetBuffer);
		}
		
		decodeRx(packetBuffer, packetSize);
		
		memset(packetBuffer,'\0',sizeof(packetBuffer));
		
		//delay(10);

		encodeTx(); 														// moved to [here]
		
	}

	//process encoder data
	newPosition = char(myEncoder.read());
	if (newPosition>=126 || newPosition<=-126)
	{
		myEncoder.write(0);
		oldPosition = -1;
		newPosition = 0;
	}
	if (newPosition != oldPosition)
	{
		if (cDebug)
		{
			Serial.print(F("PosNew:"));
			Serial.print(int(newPosition));
			Serial.print(F(", "));
			Serial.print(int(cCounter[1]));
			Serial.print(F(" PosOld:"));
			Serial.print(int(oldPosition));
			Serial.print(F(", "));
			Serial.print(int(cCounter[0]));
			Serial.println();
		}
		cCounter[0] = oldPosition;
		cCounter[1] = newPosition;
		oldPosition = newPosition;
	}

	//delay(1);
}

void decodeRx(char* rxBuffer, int iBufferLength)
{
	int iInt = 0;
	
	memset(workBuffer,'\0',sizeof(workBuffer));									// clear memory
	
	if (cDebug)
	{
		Serial.print(F("rxBuffer:"));
		for(int iTemp=0;iTemp<iBufferLength;iTemp++)
			Serial.write(rxBuffer[iTemp]);
	}

	if(rxBuffer[0] != '[')
	  return;
	
	strncpy(workBuffer,&rxBuffer[5],iBufferLength);
	if (cDebug)
	{
		Serial.print(F(" sCopy5:"));
		Serial.print(&rxBuffer[5]);
		Serial.print(F(" workBuffer:"));
		Serial.println(workBuffer);
	}
	
	/* get the first token */
	token = strtok(workBuffer, sComma);
	newToken = skip_spaces(token);
	rxTokens[iInt] = atoi(newToken);
	iInt++;
	/* walk through other tokens */
	while( token != NULL ) 
	{
		token = NULL;
		token = strtok(NULL, sComma);
		newToken = skip_spaces(token);
		rxTokens[iInt] = atoi(newToken);
		iInt++;
	}

	for (char xChar = 0; xChar < 4; xChar++)										// 0 - 3
	{
		rxChar = rxTokens[xChar];

		switch (xChar)
		{
		case 0:
			if (cDebug)
			{// get the analog output value
				//analogWrite(dacPinMap[xChar], rxChar);
				Serial.print(F(" aWr:"));
				Serial.print(dacPinMap[xChar]);
				Serial.print(F(" ~ "));
				Serial.println((byte)rxChar);
			}
			break;
		case 1:																		// get the number of output pins
			for (int zCounter = 0; zCounter < byPinsOutput; zCounter++)
			{
				uint8_t  iOutTemp = 0;
				iOutTemp = ((rxChar >> zCounter) & 1);
				if (cDebug)
				{
					Serial.print(F(" dWrite:"));
					Serial.print(pinMapOut[zCounter]);
					Serial.print(F(" ~ "));
					Serial.println(iOutTemp);
					//digitalWrite(pinMapOut[z], iOutTemp);
				}
			}
			
		
			break;
		case 2:				 														// get the values for output pins
		/*
			Serial.print(F(" oC Pins:"));
			Serial.print(rxChar);
			//Serial.print(F(" oPins:"));
			//byPinsOutput = rxChar;
			//Serial.println(byPinsOutput);
			for (byte byPinCounter = 0; byPinCounter < byPinsInput; byPinCounter++)
			{
				if(byPinCounter < byPinsOutput) 											// set only the output pins ????
				{
					Serial.print(F(" PinOutM:"));
					Serial.print(pinMap[bPinout]);
					Serial.print(F(" ~ "));
					//pinMode(pinMap[bPinout], OUTPUT);
				}
				else // set the input pins
				{
					Serial.print(F(" PinInM:"));
					Serial.print(pinMap[bPinout]);
					Serial.print(F(" ~ "));
					//pinMode(pinMap[bPinout], INPUT);
				}
			}
			Serial.println();
			*/	
			break;		
		case 3: 																	// get the pullup values for input pins
		/*
			for (int nCounter = 0; nCounter < byPins; nCounter++)
			{
				uint8_t iDirTemp = 0; 
				if (nCounter >= byPinsInput) 										// only for input pins
				{
					iDirTemp = ((rxChar >> nCounter) & 1);
					Serial.print(F(" PullUp:"));
					Serial.print(pinMap[nCounter]);
					Serial.print(F(" Dir:"));
					Serial.print(iDirTemp);
					Serial.print(F(" ~ "));
					//digitalWrite(pinMap[n], iDirTemp);
				}
			}
			Serial.println();
			*/
			break;
		}

	}
}

void encodeTx()																								// send the input pin data + encoder positions
{
	String sTest = "";
	uint8_t iDigitalStack = 0;
	for(byte byCounter = 0; byCounter < byPinsInput; byCounter++)
	{
		// int dummy = byCounter;
		if (byCounter >= 2)																					// greater than 2 are digital
		{
			//int moredummy = byCounter;
			if(digitalRead(pinMapIn[byCounter-2]))
			{
				iDigitalStack |= (1<<byCounter);
			}
		} else
		{
			adcResult = analogRead(pinMapIn[byCounter]);														// send values for analog pin
			sprintf(ReplyBuffer, "%4d", adcResult);
	
			sTest += ReplyBuffer;
			sTest +=",";
		}
	}
	
	// output the chain of pinMap 
	sprintf(ReplyBuffer,"%3d", iDigitalStack);
	
	sTest += ReplyBuffer;
	sTest += ",";
	/*
	Serial.print(F("R-PinMap buffer: "));
	Serial.print(ReplyBuffer);
	Serial.print(F(" ~ "));
	*/
	// The following is legacy encoder live state transmission
	
	sprintf(ReplyBuffer,"%4d", cCounter[0]);
	
	sTest += ReplyBuffer;
	sTest += ",";
	/*
	Serial.print(F("mpg 0: "));
	Serial.print(ReplyBuffer);
	Serial.print(F(" "));
	*/
	sprintf(ReplyBuffer,"%4d", cCounter[1]);
	sTest += ReplyBuffer;
	sTest += "~";
	
	sTest.toCharArray(ReplyBuffer,sTest.length());
	
	if (cDebug)
	{
		Serial.print(F("sTest:"));
		Serial.print(sTest);
	
		//	sTest.toCharArray(ReplyBuffer,iPacketSize);
	

		Serial.print(F(" Reply:"));
		for(int iTemp=0;iTemp<sTest.length();iTemp++)
			Serial.write(ReplyBuffer[iTemp]);
	}
	
	Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
	//Udp.beginPacket(ipRemote, portRemote);
	Udp.write(ReplyBuffer);
	Udp.endPacket();
		
}

void charClear (char * cData)
{
	for( int iCounter = 0; iCounter < sizeof(cData);  ++iCounter )
		cData[iCounter] = (char)0;
}

char *skip_spaces(const char *str)
{
	while (isspace(*str))
		++str;
	return (char *)str;
}