include SoftwareSerial char firmware_version DEBUG Set to false to dis

#include <SoftwareSerial.h>

char firmware_version[] = "0.7";

// DEBUG. Set to false to disable debug messages
bool DEBUG = false;

// Serial interfaces for output
bool USB_SERIAL_ENABLED = true; // arduino USB port
bool RS232_SERIAL_ENABLED = false; // DB9 RS-232 port

// Serial ports baud rate. Decrise if not stable link
int  USB_SERIAL_SPEED = 9600;
int  RS232_SERIAL_SPEED = 9600;

// Fire button
const int fireButtonPin = 7;
int fireButton = 0;

// 2 position zoom switch
const int zoomSwitchPin = 6;
int zoomSwitch = 0;

// 2 position fuse
const int fuseSwitchPin = 4;
int fuseSwitch = 0;

// 2 position speedSwitch
const int speedSwitchPin = 29;
int speedSwitch = 0;

// TV power on switch
  const int tvOnPin = 27;

// RS-232 pins
SoftwareSerial mySerial(11, 10); //232_TX,232_RX

// We need to send switches status while encoders not moved
// for this we need non-blocking delay
unsigned long previousMillis = 0;  // will store last time data sended
const long interval = 50;  // interval at which data sends to serial port (milliseconds)


// 6 position switch
const int modeSwitchPin1 = 8;
const int modeSwitchPin2 = 9;
const int modeSwitchPin3 = 23;
const int modeSwitchPin4 = 25;
const int modeSwitchPin5 = 12;
const int modeSwitchPin6 = 13;
int modeSwitch = 0;

// Axis X encoder
const int encoderXpinA = 2; // white encoder cable
const int encoderXpinB = 3;
volatile unsigned long tempX, counterX = 0;
unsigned int encoderXdirection = 0;

// Axis Y encoder
const int encoderYpinA = 18;
const int encoderYpinB = 19;
volatile unsigned long tempY, counterY = 0;
unsigned int encoderYdirection = 0;


void setup() {

  // set the data rate for the USB serial
  Serial.begin (USB_SERIAL_SPEED);

  // set the data rate for the SoftwareSerial RS232
  mySerial.begin(RS232_SERIAL_SPEED);

  // Open serial communications and wait for port to open
  delay(100);
  Serial.begin(USB_SERIAL_SPEED);
  while (!Serial) {
    ; // wait for serial port to connect
  }
  
  //Print firmware version, baud rate and status for serial ports
    
  Serial.println("Firmware version: " + (String)firmware_version); // to usb port
  if (USB_SERIAL_ENABLED){
    Serial.println("USB serial baud rate: " + (String)USB_SERIAL_SPEED);
  } else { 
    Serial.println("USB serial disabled");
  }
  
  mySerial.println("Firmware version: " + (String)firmware_version); // to rs232 port
  if (RS232_SERIAL_ENABLED){
    mySerial.println("RS-232 serial baud rate: " + (String)RS232_SERIAL_SPEED);
  } else {
    mySerial.println("RS-232 serial disabled");
  }
  
  // initialize pins as pullup input
  pinMode(fireButtonPin, INPUT_PULLUP);
  pinMode(zoomSwitchPin, INPUT_PULLUP);
  pinMode(fuseSwitchPin, INPUT_PULLUP);
  pinMode(speedSwitchPin, INPUT_PULLUP);
  pinMode(modeSwitchPin1, INPUT_PULLUP);
  pinMode(modeSwitchPin2, INPUT_PULLUP);
  pinMode(modeSwitchPin3, INPUT_PULLUP);
  pinMode(modeSwitchPin4, INPUT_PULLUP);
  pinMode(modeSwitchPin5, INPUT_PULLUP);
  pinMode(modeSwitchPin6, INPUT_PULLUP);

  

  // Axis X Encoder
  pinMode(encoderXpinA, INPUT_PULLUP);
  pinMode(encoderXpinB, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(encoderXpinA), encXinteruptA, RISING);
  attachInterrupt(digitalPinToInterrupt(encoderXpinB), encXinteruptB, RISING);

  // Axis Y Encoder
  pinMode(encoderYpinA, INPUT_PULLUP);
  pinMode(encoderYpinB, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(encoderYpinA), encYinteruptA, RISING);
  attachInterrupt(digitalPinToInterrupt(encoderYpinB), encYinteruptB, RISING);

  // Turn TV on
  pinMode(tvOnPin, OUTPUT);
  digitalWrite(tvOnPin, HIGH);
  delay(1000);
  digitalWrite(tvOnPin, LOW);

}

void SendData(int axisX = 0, int axisY = 0) {
   
   // Built-in Arduino USB serial
   if (USB_SERIAL_ENABLED) {
   Serial.println (
    (String)axisX
    + (String)axisY
    + modeSwitch
    + zoomSwitch
    + fuseSwitch
    + fireButton
    + speedSwitch
  );
  }

  // RS-232 serial
  if (RS232_SERIAL_ENABLED) {
  mySerial.println(
   (String)axisX
   + (String)axisY
   + modeSwitch
   + zoomSwitch
   + fuseSwitch
   + fireButton
   + speedSwitch
  );
  }
  
}


// Encoder X interrupts
void encXinteruptA() {
  if (digitalRead(encoderXpinB) == LOW) {
    counterX++;
  } else {
    counterX--;
  }
}

void encXinteruptB() {
  if (digitalRead(encoderXpinA) == LOW) {
    counterX--;
  } else {
    counterX++;
  }
}

// Encoder Y interrupts
void encYinteruptA() {
  if (digitalRead(encoderYpinB) == LOW) {
    counterY++;
  } else {
    counterY--;
  }
}

void encYinteruptB() {
  if (digitalRead(encoderYpinA) == LOW) {
    counterY--;
  } else {
    counterY++;
  }
}

void loop() {

  // Fire button. LOW if pressed, HIGH if not pressed
  if (digitalRead(fireButtonPin) == LOW) {
    fireButton = 1;
  } else {
    fireButton = 0;
  }

  // Zoom switch. LOW if X1 state, HIGH if X10 state
  if (digitalRead(zoomSwitchPin) == LOW) {
    zoomSwitch = 1;
  } else {
    zoomSwitch = 0;
  }

  // Fuse switch. LOW if deactivated state, HIGH if active
  if (digitalRead(fuseSwitchPin) == LOW) {
    fuseSwitch = 1;
  } else {
    fuseSwitch = 0;
  }

   // Speed switch. LOW if deactivated state, HIGH if active
  if (digitalRead(speedSwitchPin) == LOW) {
    speedSwitch = 1;
  } else {
    speedSwitch = 0;
  }

  // 6 position switch
  if (digitalRead(modeSwitchPin1) == LOW) {
    modeSwitch = 1;
  }
  if (digitalRead(modeSwitchPin2) == LOW) {
    modeSwitch = 2;
  }
  if (digitalRead(modeSwitchPin3) == LOW) {
    modeSwitch = 3;
  }
  if (digitalRead(modeSwitchPin4) == LOW) {
    modeSwitch = 4;
  }
  if (digitalRead(modeSwitchPin5) == LOW) {
    modeSwitch = 5;
  }
  if (digitalRead(modeSwitchPin6) == LOW) {
    modeSwitch = 6;
  }

  // Encoder X routine
  if ( counterX != tempX ) {
    if (DEBUG){Serial.println ((String)"Encoder X: "+counterX);}
    if (counterX > tempX) {
      encoderXdirection = 1;
    }
    else {
      encoderXdirection = 2;
    }
  } else {
    encoderXdirection = 0;
  }

  // Encoder Y routine
  if ( counterY != tempY ) {
    if (DEBUG){Serial.println ((String)"Encoder Y: "+counterY);}
    if (counterY > tempY) {
      encoderYdirection = 1;
    }
    else {
      encoderYdirection = 2;
    }
  } else {
    encoderYdirection = 0;
  }

  //
  // Sending data to serial port
  //

/*
// if encoders moved
  if ( counterX != tempX || counterY != tempY) {
    //for (int i=min(tempX, counterX); i<max(tempX, counterX); i++) {
      //SendData(encoderXdirection, 0);
    //}
    //for (int i=min(tempY, counterY); i<max(tempY, counterY); i++) {
      //SendData(0, encoderYdirection);
   SendData(encoderXdirection,encoderYdirection);
  }
  // if encoders still
  else {
    SendData();
  }

*/
  // check if it's time to send data.
  // If the difference between the current time and last time when we send data
  unsigned long currentMillis = millis();

  // send data if its right time
  if (currentMillis - previousMillis >= interval) {
    // save the last time we send data
    previousMillis = currentMillis;
    SendData();
  }

  // Reset counters when sended
  // Encoders counters set to zero
  tempX = counterX;
  tempY = counterY;



} // end of main loop