How do I get rid of duplication

Projects Programming
31

Final code (I hope). Relays didn't like being in the servo loop. Separated them and found a couple of data errors and all is working smoothly.

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <CMRI.h>
#define MIN_PULSE_WIDTH       95
#define MAX_PULSE_WIDTH       530
#define FREQUENCY             50

Adafruit_PWMServoDriver Pwm[2] =  {
  Adafruit_PWMServoDriver(0x40),
  Adafruit_PWMServoDriver(0x41)
};
CMRI cmri(0, 96, 32);     //CPNODE with address 0, 96 inputs, 32 outputs

struct turnoutData
{
  byte pwm;                  //selects PWM board 0 or 1
  int  targetPos;            //based on cmri.get_bit, set to either pos1 or pos2
  int  pos1;                 //servo position 1
  int  pos2;                 //servo position 2
  int  offset;               //horn offset from 90 degree position added to targetPos before sending to servo
  byte relayPin;
  byte relayState;           //set to either relayState1 or !relayState1 based on cmri.get_bit value
  byte relayState1;          //servo's corresponding relays position 1 state
  int  currentPos;           //used to use slow motion to move servo from position to position
};

struct turnoutData turnArray[] = {         //pwm; targetPos; pos1; pos2; offset; relayPin; relayState; relayState1; currentPos;
  { 0, 312, 249, 375, -40, 35, 0, 1, 312}, //15
  { 0, 312, 249, 375,  20, 67, 0, 1, 312}, //12
  { 0, 312, 375, 249,  -9, 30, 1, 0, 312}, //21
  { 0, 312, 249, 375,  12, 68, 1, 0, 312}, //13
  { 0, 312, 249, 375,   0, 32, 0, 1, 312}, //14
  { 0, 312, 249, 375,  14, 33, 0, 1, 312}, //20
  { 0, 312, 312, 312,   0, 70, 0, 0, 312}, //Future use (will be deleted when computer side is updated)
  { 0, 312, 312, 312,   0, 70, 0, 0, 312}, //Future use
  { 0, 312, 312, 312,   0, 70, 0, 0, 312}, //Future use
  { 0, 312, 375, 249,   2, 69, 0, 1, 312}, //19
  { 0, 312, 375, 249,   6, 64, 1, 0, 312}, //9
  { 0, 312, 375, 249,   9, 66, 1, 0, 312}, //11
  { 0, 312, 375, 249, -31, 65, 0, 1, 312}, //10
  { 0, 312, 375, 249,  18, 63, 1, 0, 312}, //4
  { 0, 312, 375, 249,  -9, 62, 0, 1, 312}, //3
  { 0, 312, 375, 249,   0, 31, 0, 1, 312}, //18

  { 1, 312, 249, 375, -19, 23, 1, 0, 312}, //1
  { 1, 312, 249, 375,   6, 25, 0, 1, 312}, //5
  { 1, 312, 249, 375, -23, 29, 1, 0, 312}, //16
  { 1, 312, 249, 375, -16, 22, 0, 1, 312}, //2
  { 1, 312, 249, 375,  -6, 28, 0, 1, 312}, //17
  { 1, 312, 249, 375,   0, 24, 1, 0, 312}, //6
  { 1, 312, 375, 249,   0, 27, 0, 1, 312}, //7
  { 1, 312, 249, 375,  -1, 26, 1, 0, 312}, //8
  { 1, 312, 375, 249,  -9, 34, 0, 1, 312}  //22
};

const int turnArrayLength = sizeof (turnArray) / sizeof (turnArray[0]);

void processTurnouts() {
  for (int i = 0; i < turnArrayLength; i++) {
    turnArray[i].targetPos = (cmri.get_bit(i) == 1) ? turnArray[i].pos1 : turnArray[i].pos2;
    if (turnArray[i].targetPos != turnArray[i].currentPos) {
      if (turnArray[i].targetPos > turnArray[i].currentPos) {
        turnArray[i].currentPos++;
        if (turnArray[i].currentPos >= turnArray[i].targetPos) {
          turnArray[i].currentPos = turnArray[i].targetPos;
        }
        Pwm[turnArray[i].pwm].setPWM(i % 16, 0, turnArray[i].currentPos + turnArray[i].offset);
      }
      else {
        turnArray[i].currentPos--;
        if (turnArray[i].currentPos <= turnArray[i].targetPos) {
          turnArray[i].currentPos = turnArray[i].targetPos;
        }
        Pwm[turnArray[i].pwm].setPWM(i % 16, 0, turnArray[i].currentPos + turnArray[i].offset);
      }
    }
  }
}

void processRelays() {
  for (int i = 0; i < turnArrayLength; i++) {
    turnArray[i].relayState = (turnArray[i].currentPos == turnArray[i].pos1) ? turnArray[i].relayState1 : !turnArray[i].relayState1;
    cmri.set_bit( i , turnArray[i].relayState );
    digitalWrite(turnArray[i].relayPin, turnArray[i].relayState);
  }
}

void setup() {
  Serial.begin(57600);
  Serial.println("JMRI CMRI Arduino Mega Interface");
  Wire.setClock(400000);
  for (byte i = 0; i < 2; i++) {
    Pwm[i].begin();
    Pwm[i].setPWMFreq(FREQUENCY);
  }
  for (byte i = 0; i < turnArrayLength; i++) {
    pinMode(turnArray[i].relayPin, OUTPUT);
    digitalWrite(turnArray[i].relayPin, turnArray[i].relayState);
  }
  Serial.println("Init completed.");
}

void loop() {
  cmri.process();
  processTurnouts();
  processRelays();
}

Thanks for everyone's patience and help streamlining this code!

Mandy