How do I get rid of duplication

Projects Programming
26

Okay, so my head hurts a little bit... This is where I'm at, I haven't uploaded to the Mega yet. Code compiles and I think will work although mistakes are always possible.

#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[NUMSERVOS] = {          //servos on pwm0 board position and relay data
  { 0, 312, 249, 375, -40, 35, 0, 1, 312},           //312 is 90 degrees,  249  is 70 degrees, 375 is 110 degrees
  { 0, 312, 249, 375,  20, 67, 0, 1, 312},
  { 0, 312, 375, 249,  -9, 30, 1, 0, 312},
  { 0, 312, 249, 375,  12, 68, 0, 0, 312},
  { 0, 312, 249, 375,   0, 32, 0, 1, 312},
  { 0, 312, 249, 375,  14, 33, 0, 1, 312},
  { 0, 312, 312, 312,   0, 70, 0, 0, 312}, //Future use
  { 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},
  { 0, 312, 375, 249,   6, 64, 1, 0, 312},
  { 0, 312, 375, 249,   9, 66, 1, 0, 312},
  { 0, 312, 375, 249, -31, 65, 0, 1, 312},
  { 0, 312, 375, 249,  18, 63, 1, 0, 312},
  { 0, 312, 375, 249,  -9, 62, 0, 1, 312},
  { 0, 312, 375, 249,   0, 31, 0, 1, 312},
  { 1, 312, 249, 375, -19, 23, 1, 0, 312},
  { 1, 312, 249, 375,   6, 25, 0, 1, 312},
  { 1, 312, 249, 375, -23, 29, 1, 0, 312},
  { 1, 312, 249, 375, -16, 22, 0, 1, 312},
  { 1, 312, 249, 375,  -6, 28, 0, 1, 312},
  { 1, 312, 249, 375,   0, 24, 1, 0, 312},
  { 1, 312, 375, 249,   0, 27, 0, 1, 312},
  { 1, 312, 249, 375,  -1, 26, 1, 0, 312},
  { 1, 312, 375, 249,  -9, 34, 0, 1, 312}
//  { 1, 312, 312, 312,   0, 70, 0, 0, 312}, //Future use
//  { 1, 312, 312, 312,   0, 70, 0, 0, 312}, //Future use
//  { 1, 312, 312, 312,   0, 70, 0, 0, 312}, //Future use
//  { 1, 312, 312, 312,   0, 70, 0, 0, 312}, //Future use
//  { 1, 312, 312, 312,   0, 70, 0, 0, 312}, //Future use
//  { 1, 312, 312, 312,   0, 70, 0, 0, 312}, //Future use
//  { 1, 312, 312, 312,   0, 70, 0, 0, 312}  //Future use
};
const int turnArrayLength = sizeof (turnArray) / sizeof (turnArray[0]);

void processTurnouts() {
  for (int i = 0; i < turnArrayLength; i++) {

    (cmri.get_bit(i) == 1) ? (turnArray[i].targetPos = turnArray[i].pos1) : (turnArray[i].targetPos = turnArray[i].pos2);
    if (turnArray[i].targetPos != turnArray[i].currentPos) {
      if (turnArray[i].targetPos > turnArray[i].currentPos) {
        turnArray[i].currentPos += 2;
        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 -= 2;
        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);
      }
    }

    (turnArray[i].currentPos == turnArray[i].pos1) ? (turnArray[i].relayState = turnArray[i].relayState1) : (turnArray[i].relayState = !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();
}

Thanks for looking,

Mandy