Updated code, took out a couple of for loops. Still wondering if there is a way to consolidate turnArray0, and pwm0 (lines 67 to 83) with turnArray1 and pwm 1 (lines 85 to 101).
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <CMRI.h>
#define MIN_PULSE_WIDTH 95
#define MAX_PULSE_WIDTH 530
#define FREQUENCY 50
#define NUMSERVOS 16
Adafruit_PWMServoDriver pwm0 = Adafruit_PWMServoDriver(0x40);
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x41);
CMRI cmri(0, 96, 32); //CPNODE with address 0, 96 inputs, 32 outputs
struct turnoutData
{
int targetPos; //based on getbit, 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 relayState2 based on Tbit value
byte relayState1; //servo's corresponding relays position 1 state
byte relayState2; //servo's corresponding relays position 2 state
int currentPos; //used to use slow motion to move servo from position to position
};
struct turnoutData turnArray0[] = { //servos on pwm0 board position and relay data
{ 312, 249, 375, -40, 35, 0, 1, 0, 312}, //312 is 90 degrees, 249 is 70 degrees, 375 is 110 degrees
{ 312, 249, 375, 20, 67, 0, 1, 0, 312},
{ 312, 375, 249, -9, 30, 1, 0, 1, 312},
{ 312, 249, 375, 12, 68, 0, 0, 1, 312},
{ 312, 249, 375, 0, 32, 0, 1, 0, 312},
{ 312, 249, 375, 14, 33, 0, 1, 0, 312},
{ 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 312, 375, 249, 2, 69, 0, 1, 0, 312},
{ 312, 375, 249, 6, 64, 1, 0, 1, 312},
{ 312, 375, 249, 9, 66, 1, 0, 1, 312},
{ 312, 375, 249, -31, 65, 0, 1, 0, 312},
{ 312, 375, 249, 18, 63, 1, 0, 1, 312},
{ 312, 375, 249, -9, 62, 0, 1, 0, 312},
{ 312, 375, 249, 0, 31, 0, 1, 0, 312},
};
struct turnoutData turnArray1[] = { //servos on pwm1 board position and relay data
{ 312, 249, 375, -19, 23, 1, 0, 1, 312},
{ 312, 249, 375, 6, 25, 0, 1, 0, 312},
{ 312, 249, 375, -23, 29, 1, 0, 1, 312},
{ 312, 249, 375, -16, 22, 0, 1, 0, 312},
{ 312, 249, 375, -6, 28, 0, 1, 0, 312},
{ 312, 249, 375, 0, 24, 1, 0, 1, 312},
{ 312, 375, 249, 0, 27, 0, 1, 0, 312},
{ 312, 249, 375, -1, 26, 1, 0, 1, 312},
{ 312, 375, 249, -9, 34, 0, 1, 0, 312},
{ 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
};
void processTurnouts() {
for (int i = 0; i < NUMSERVOS; i++) {
(cmri.get_bit(i) == 1) ? (turnArray0[i].targetPos = turnArray0[i].pos1) : (turnArray0[i].targetPos = turnArray0[i].pos2);
if (turnArray0[i].targetPos != turnArray0[i].currentPos) {
if (turnArray0[i].targetPos > turnArray0[i].currentPos) {
turnArray0[i].currentPos+=2;
if (turnArray0[i].currentPos >= turnArray0[i].targetPos) {
turnArray0[i].currentPos = turnArray0[i].targetPos;
}
pwm0.setPWM(i, 0, turnArray0[i].currentPos + turnArray0[i].offset);
}
else {
turnArray0[i].currentPos-=2;
if (turnArray0[i].currentPos <= turnArray0[i].targetPos) {
turnArray0[i].currentPos = turnArray0[i].targetPos;
}
pwm0.setPWM(i, 0, turnArray0[i].currentPos + turnArray0[i].offset);
}
}
(cmri.get_bit( i + NUMSERVOS ) == 1) ? (turnArray1[i].targetPos = turnArray1[i].pos1) : (turnArray1[i].targetPos = turnArray1[i].pos2);
if (turnArray1[i].targetPos != turnArray1[i].currentPos) {
if (turnArray1[i].targetPos > turnArray1[i].currentPos) {
turnArray1[i].currentPos+=2;
if (turnArray1[i].currentPos >= turnArray1[i].targetPos) {
turnArray1[i].currentPos = turnArray1[i].targetPos;
}
pwm1.setPWM(i, 0, turnArray1[i].currentPos + turnArray1[i].offset);
}
else {
turnArray1[i].currentPos-=2;
if (turnArray1[i].currentPos <= turnArray1[i].targetPos) {
turnArray1[i].currentPos = turnArray1[i].targetPos;
}
pwm1.setPWM(i, 0, turnArray1[i].currentPos + turnArray1[i].offset);
}
}
(turnArray1[i].currentPos == turnArray1[i].pos1) ? (turnArray1[i].relayState = turnArray1[i].relayState1) : (turnArray1[i].relayState = turnArray1[i].relayState2);
(turnArray0[i].currentPos == turnArray0[i].pos1) ? (turnArray0[i].relayState = turnArray0[i].relayState1) : (turnArray0[i].relayState = turnArray0[i].relayState2);
cmri.set_bit( i , turnArray0[i].relayState );
cmri.set_bit( (i + NUMSERVOS) , turnArray1[i].relayState );
digitalWrite(turnArray0[i].relayPin, turnArray0[i].relayState);
digitalWrite(turnArray1[i].relayPin, turnArray1[i].relayState);
}
}
void setup() {
Serial.begin(57600);
Serial.println("JMRI CMRI Arduino Mega Interface");
Wire.setClock(400000);
pwm0.begin();
pwm0.setPWMFreq(FREQUENCY);
pwm1.begin();
pwm1.setPWMFreq(FREQUENCY);
for (byte i = 0; i < NUMSERVOS; i++) {
pinMode(turnArray0[i].relayPin, OUTPUT);
digitalWrite(turnArray0[i].relayPin, turnArray0[i].relayState);
pinMode(turnArray1[i].relayPin, OUTPUT);
digitalWrite(turnArray1[i].relayPin, turnArray1[i].relayState);
}
Serial.println("Init completed.");
}
void loop() {
cmri.process();
processTurnouts();
}
Thanks,
Mandy