diff --git a/LinuxCNC_ArduinoConnector.ino b/LinuxCNC_ArduinoConnector.ino index e98afc1..a341171 100644 --- a/LinuxCNC_ArduinoConnector.ino +++ b/LinuxCNC_ArduinoConnector.ino @@ -1,24 +1,53 @@ /* -This Software is used as IO Expansion for LinuxCNC. Here i am using an Mega 2560. -It can use as many digital & analog Inputs, Outputs and PWM Outputs as your Arduino can handle. -I also generate "virtual Pins" by using latching Potentiometers, which are connected to one analog Pin, but are read in Hal as individual Pins. + LinuxCNC_ArduinoConnector + By Alexander Richter, info@theartoftinkering.com 2022 + This Software is used as IO Expansion for LinuxCNC. Here i am using a Mega 2560. -The Send Protocol is : + It is NOT intended for timing and security relevant IO's. Don't use it for Emergency Stops or Endstop switches! -Inputs are encoded with Letter 'I' -Keep alive Signal is send with Letter 'E' -Outputs are encoded with Letter 'O' -PWM Outputs are encoded with Letter 'P' -Analog Inputs are encoded with Letter 'A' -Latching Potentiometers are encoded with Letter 'L' -Absolute Encoder input is encoded with Letter 'K' + You can create as many digital & analog Inputs, Outputs and PWM Outputs as your Arduino can handle. + You can also generate "virtual Pins" by using latching Potentiometers, which are connected to one analog Pin, but are read in Hal as individual Pins. + + Currently the Software provides: + - analog Inputs + - latching Potentiometers + - 1 absolute encoder input + - digital Inputs + - digital Outputs + + The Send and receive Protocol is : + To begin Transmitting Ready is send out and expects to receive E: to establish connection. Afterwards Data is exchanged. + Data is only send everythime it changes once. + + Inputs = 'I' -write only -Pin State: 0,1 + Outputs = 'O' -read only -Pin State: 0,1 + PWM Outputs = 'P' -read only -Pin State: 0-255 + Analog Inputs = 'A' -write only -Pin State: 0-1024 + Latching Potentiometers = 'L' -write only -Pin State: 0-max Position + Absolute Encoder input = 'K' -write only -Pin State: 0-32 + + Command 'E0:0' is used for connectivity checks and is send every 5 seconds as keep alive signal + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + See the GNU General Public License for more details. + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ + + //###IO's### -#define INPUTS -#ifdef INPUTS +#define DINPUTS +#ifdef DINPUTS const int Inputs = 16; //number of inputs using internal Pullup resistor. (short to ground to trigger) int InPinmap[] = {32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48}; #endif @@ -32,23 +61,25 @@ Absolute Encoder input is encoded with Letter 'K' #define PWMOUTPUTS #ifdef PWMOUTPUTS - const int PwmOutput = 2; //number of outputs - int PwmOutPinmap[] = {12,11}; + const int PwmOutputs = 2; //number of outputs + int PwmOutPinmap[] = {13,11}; #endif #define AINPUTS #ifdef AINPUTS const int AInputs = 1; int AInPinmap[] = {A3}; //Potentiometer for SpindleSpeed override + int smooth = 200; //number of samples to denoise ADC, try lower numbers on your setup #endif #define LPOTIS #ifdef LPOTIS const int LPotis = 2; int LPotiPins[LPotis][2] = { - {A1,8}, //Latching Knob Spindle Overdrive on A1, has 9 Positions - {A2,3} //Latching Knob Feed Resolution on A2, has 4 Positions + {96,8}, //Latching Knob Spindle Overdrive on A1, has 9 Positions + {95,3} //Latching Knob Feed Resolution on A2, has 4 Positions }; + int margin = 20; //giving it some margin so Numbers dont jitter, make this number smaller if your knob has more than 50 Positions #endif #define ABSENCODER @@ -70,7 +101,7 @@ const int timeout = 10000; // timeout after 10 sec not receiving Stuff #define DEBUG //Variables for Saving States -#ifdef INPUTS +#ifdef DINPUTS int InState[Inputs]; int oldInState[Inputs]; #endif @@ -78,10 +109,15 @@ const int timeout = 10000; // timeout after 10 sec not receiving Stuff int OutState[Outputs]; int oldOutState[Outputs]; #endif +#ifdef PWMOUTPUTS + int OutPWMState[PwmOutputs]; + int oldOutPWMState[PwmOutputs]; +#endif #ifdef AINPUTS int oldAinput[AInputs]; #endif #ifdef LPOTIS + int Lpoti[LPotis]; int oldLpoti[LPotis]; #endif #ifdef ABSENCODER @@ -91,7 +127,7 @@ const int timeout = 10000; // timeout after 10 sec not receiving Stuff //### global Variables setup### -//Diese variablen nicht von außen anfassen +//Please don't touch them unsigned long oldmillis = 0; unsigned long newcom = 0; unsigned long lastcom = 0; @@ -109,8 +145,39 @@ uint16_t io = 0; uint16_t value = 0; + void setup() { +#ifdef DINPUTS +//setting Inputs with internal Pullup Resistors + for(int i= 0; i timeout){ - StatLedErr(); + StatLedErr(1000,10); + } + else{ + digitalWrite(StatLedPin, HIGH); } } -void StatLedErr(){ +void StatLedErr(int offtime, int ontime){ unsigned long newMillis = millis(); - if (newMillis - oldmillis >= StatLedErrDel[0]){ + if (newMillis - oldmillis >= offtime){ digitalWrite(StatLedPin, HIGH); } - if (newMillis - oldmillis >= StatLedErrDel[0]+StatLedErrDel[1]){{ + if (newMillis - oldmillis >= offtime+ontime){{ digitalWrite(StatLedPin, LOW); oldmillis = newMillis; } @@ -207,57 +260,55 @@ void flushSerial(){ Serial.read(); } } -/* -void readData(){ - int pin = 0; - int state = 0; - byte terminated = false; - if (Serial.available() > 0) { - - char inChar = Serial.read(); - Serial.println(inChar); - if (inChar == 'o'){ - Serial.println("O erkannt"); - while (!terminated && comalive()){ - - inChar = Serial.read(); - if (inChar == ':'){ +void writeOutputs(int Pin, int Stat){ + for(int x = 0; x: +# To begin Transmitting Ready is send out and expects to receive E: to establish connection. Afterwards Data is exchanged. +# Data is only send everythime it changes once. + +# Inputs = 'I' -write only -Pin State: 0,1 +# Outputs = 'O' -read only -Pin State: 0,1 +# PWM Outputs = 'P' -read only -Pin State: 0-255 +# Analog Inputs = 'A' -write only -Pin State: 0-1024 +# Latching Potentiometers = 'L' -write only -Pin State: 0-max Position +# Absolute Encoder input = 'K' -write only -Pin State: 0-32 + + +# Command 'E0:0' is used for connectivity checks and is send every 5 seconds as keep alive signal + +# This program is free software; you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation; either version 2 of the License, or +# (at your option) any later version. +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +# See the GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + c = hal.component("arduino") #name that we will cal pins from in hal +connection = '/dev/ttyACM0' + # Set how many Inputs you have programmed in Arduino and which pins are Inputs Inputs = 17 @@ -60,6 +76,7 @@ AbsKnobPos = 30 ######## End of Config! ######## + ######## SetUp of HalPins ######## # setup Input halpins @@ -118,7 +135,7 @@ def extract_nbr(input_str): ######## Detect Serial connection and blink Status LED if connection lost -todo ######## #try: -arduino = serial.Serial('/dev/ttyACM0', 9600, timeout=1, xonxoff=False, rtscts=False, dsrdtr=True) +arduino = serial.Serial(connection, 9600, timeout=1, xonxoff=False, rtscts=False, dsrdtr=True) while True: @@ -139,18 +156,14 @@ while True: if data[0] == "I": c.dIn = data[1] - elif data[0] == "aI": + elif data[0] == "A": c.aIn = data[1] - elif data[0] == "lP": + elif data[0] == "L": for port in range(LPotiLatches[latches]): if ("LPoti-%02d %" [port]) == data[1]: - #s - else: - c.("LPoti-%02d %" [port]) = 0 - - c.LPotiKnob = data[1] - elif data[0] == "AK": + + elif data[0] == "K": c.AbsKnob = data[1] else: pass