i know the following may be hard to follow because of the simulation code and limited # of entries, but look at the changes to processTurnout()
#include <Wire.h>
#define FREQUENCY 50
char s [90];
// -----------------------------------------------------------------------------
#define MyHW
#ifdef MyHW
struct CMRI {
int bits;
CMRI (int adr, int nInp, int nOut) { bits = 0; };
void process (void) {
if (Serial.available()) {
int n = Serial.readBytesUntil ('\n', s, sizeof(s));
s [n] = 0;
bits = atoi(s);
Serial.println (bits);
}
};
int get_bit (int bit) {
int val = (bits >> bit) & 1;
sprintf (s, "%s: bits 0x%02x, bit %2d, val 0x%02x",
__func__, bits, bit, val);
// Serial.println (s);
return (val);
};
void set_bit (int bit, int val) { };
};
// -------------------------------------
struct Adafruit_PWMServoDriver {
int _adr;
Adafruit_PWMServoDriver (int adr) { _adr = adr; };
void begin (void) { };
void setPWM (int id, int x, int pos) {
sprintf (s, "%s: adr %2d, pos %3d", __func__, _adr, pos);
Serial.println (s);
analogWrite (_adr, pos);
};
void setPWMFreq (int freq) { };
};
Adafruit_PWMServoDriver pwm0 = Adafruit_PWMServoDriver (10);
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver (11);
// ---------------------------------------------------------
#else
#include <Adafruit_PWMServoDriver.h>
#include <CMRI.h>
Adafruit_PWMServoDriver pwm0 = Adafruit_PWMServoDriver(0x40);
Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x41);
#endif
// -----------------------------------------------------------------------------
struct turnoutData
{
Adafruit_PWMServoDriver *pPwm;
int targetPos; //based on Tbit, 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
byte Tbit; //0 or 1 from external program
};
// ---------------------------------------------------------
#ifdef MyHW
struct turnoutData turnArray0 [] = {
{ &pwm0, 150, 255, 150, 0, 13, 0, 1, 0, 160},
{ &pwm1, 150, 255, 150, 0, 12, 0, 1, 0, 200},
};
// -------------------------------------
#elif
struct turnoutData turnArray0[] = {
{ 0, 312, 249, 375, -40, 35, 0, 1, 0, 312}, //312 is 90 degrees, 249 is 70 degrees, 375 is 110 degrees
{ 0, 312, 249, 375, 20, 67, 0, 1, 0, 312},
{ 0, 312, 375, 249, -9, 30, 1, 0, 1, 312},
{ 0, 312, 249, 375, 12, 68, 0, 0, 1, 312},
{ 0, 312, 249, 375, 0, 32, 0, 1, 0, 312},
{ 0, 312, 249, 375, 14, 33, 0, 1, 0, 312},
{ 0, 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 0, 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 0, 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 0, 312, 375, 249, 2, 69, 0, 1, 0, 312},
{ 0, 312, 375, 249, 6, 64, 1, 0, 1, 312},
{ 0, 312, 375, 249, 9, 66, 1, 0, 1, 312},
{ 0, 312, 375, 249, -31, 65, 0, 1, 0, 312},
{ 0, 312, 375, 249, 18, 63, 1, 0, 1, 312},
{ 0, 312, 375, 249, -9, 62, 0, 1, 0, 312},
{ 0, 312, 375, 249, 0, 31, 0, 1, 0, 312},
};
struct turnoutData turnArray1[] = {
{ 0, 312, 249, 375, -19, 23, 1, 0, 1, 312},
{ 0, 312, 249, 375, 6, 25, 0, 1, 0, 312},
{ 0, 312, 249, 375, -23, 29, 1, 0, 1, 312},
{ 0, 312, 249, 375, -16, 22, 0, 1, 0, 312},
{ 0, 312, 249, 375, -6, 28, 0, 1, 0, 312},
{ 0, 312, 249, 375, 0, 24, 1, 0, 1, 312},
{ 0, 312, 375, 249, 0, 27, 0, 1, 0, 312},
{ 0, 312, 249, 375, -1, 26, 1, 0, 1, 312},
{ 0, 312, 375, 249, -9, 34, 0, 1, 0, 312},
{ 0, 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 0, 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 0, 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 0, 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 0, 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 0, 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
{ 0, 312, 312, 312, 0, 70, 0, 0, 0, 312}, //Future use
};
#endif
// -----------------------------------------------------------------------------
#define DELTA 2
#define N_SERVO (sizeof(turnArray0)/sizeof(turnoutData))
CMRI cmri(0, 96, 32); //CPNODE with address 0, 96 inputs, 32 outputs
// -----------------------------------------------------------------------------
void processTurnouts (
turnoutData *t )
{
for (unsigned i = 0; i < N_SERVO; i++, t++) {
(t->Tbit == 1)
? (t->targetPos = t->pos1)
: (t->targetPos = t->pos2);
if (t->targetPos != t->currentPos) {
sprintf (s, " %s: %2d %4d <-- %4d", __func__, i,
t->targetPos, t->currentPos);
// Serial.println (s);
int delta = t->targetPos - t->currentPos;
delta = DELTA < delta ? DELTA : (-DELTA > delta ? -DELTA : delta);
t->currentPos += delta;
t->pPwm->setPWM(i, 0, t->currentPos + t->offset);
}
(t->currentPos == t->pos1)
? (t->relayState = t->relayState1)
: (t->relayState = t->relayState2);
digitalWrite(t->relayPin, t->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 (unsigned i = 0; i < N_SERVO; i++) {
pinMode(turnArray0[i].relayPin, OUTPUT);
digitalWrite(turnArray0[i].relayPin, turnArray0[i].relayState);
}
Serial.println("Init completed.");
}
// -----------------------------------------------------------------------------
void loop() {
cmri.process();
for (unsigned i = 0; i < N_SERVO; i++) {
turnArray0[i].Tbit = cmri.get_bit(i); // update bit from c/mri
}
processTurnouts (turnArray0);
for (unsigned i = 0; i < N_SERVO; i++) {
cmri.set_bit( i , turnArray0[i].relayState ); //send relayStates back to computer
}
delay (100);
}