Here's all the code as requested.
// Wire Slave Receiver No 2
// point status 1 = straight; 2 - turn out; 0 = failed
#include <Wire.h>
#include <HCPCA9685.h>
#include <SPI.h>
#include <NRFLite.h>
#include <EEPROM.h>
#define I2CAdd 0x40
HCPCA9685 Myservo(I2CAdd);
const static byte slave_ID = 2;
const static byte MasterradioID = 0;
const static byte CE = 9;
const static byte CSN = 10;
struct RadioPacket {// Any packet up to 32 bytes can be sent.
byte FromRadioId;
byte pushbuttonNumber;
bool didchange;
byte pointStatus;
};
NRFLite slaveRadio;
RadioPacket _radioData;
int currentpointPos =1; // 1=straight, 2 = turnout
int currentposdoubleSlip =1; //positions are 1, 2, 3, 4
byte straightMSPin[] = {23,24,29,27,30,33,35,37,39};
byte turnoutMSPin[] = {22,25,28,26,31,32,34,36,38};
byte DS1straightPin = 40;
byte DS1turnoutPin = 41;
byte DS2straightPin = 42;
byte DS2turnoutPin = 43;
int DS1 = 9; //servo output number for doubleslip
int DS2 = 10;
int DS1servoHome = 10;
int DS1servoTurnout = 120;
int DS2servoHome = 10;
int DS2servoTurnout = 120;
// original servo home and turnout positions saved to EEPROM
int servoHomePos[]= {1, 10, 160, 120, 5, 150, 130, 10, 10};
int servoTurnoutPos[]= {140, 120, 10, 10, 170, 10, 10, 140, 120};
int button2servomap[]={8, 9, 11, 12, 13, 14, 16, 17, 18, 15, 15};
int whichServo = 0;
int whichButton = 0;
byte currentPosition[9];
bool didpointChange = false;
byte myPos;
bool straightMS = false;
bool turnoutMS = false;
void setup(){
/* Initialise the library and set it to 'servo mode' */
Myservo.Init(SERVO_MODE);
/* Wake the device up */
Myservo.Sleep(false);
Serial.begin(115200); // start serial for output
// set pins to Inputs
for (int x =22; x<45; x++){
pinMode(x, INPUT);
}
/* //read status all points except double slip
for (int y = 0; y<8; y++){
if (digitalRead(straightMSPin[y]) == HIGH){
lhMicro[y] = HIGH;
currentPosition[y] = 1; //straight. assumes lhmicro is high when straight.
}else {
lhMicro[y] = LOW;
}
if (digitalRead(turnoutMSPin[y]) == HIGH){
rhMicro[y] = HIGH;
currentPosition[y] = 2; // turnout
}else{
rhMicro[y] = LOW;
}
}*/
// write homepos to EEPROM servo values. 0 -10 LH; 11-21 RH
/* for (int z = 0; z <11; z++){
EEPROM.write(z, servoHomePos[z]);
}
for (int z =11; z<22; z++){
EEPROM.write(z, servoTurnoutPos[z-11]);
}delay(2000);
//read servo home and turnout positions from EEPROM
for (int z =0; z<11; z++){
myPos = EEPROM.read(z);
//Serial.println(myPos);
servoHomePos[z] = myPos;
}*/
for (int z = 11; z<22; z++){
myPos = EEPROM.read(z);
servoTurnoutPos[z-11] = myPos;
}
if (!slaveRadio.init(slave_ID, CE, CSN)){
Serial.println("Cannot communicate with radio");
while (1); // Wait here forever.
}else{
Serial.println("Radio OK");
}
for (int z= 0; z<9; z++){
currentPosition[z]=1; //straight
}
// set all points to straight
for (int p =0; p<11; p++){
Myservo.Servo(p, servoHomePos[p]);
delay(500);
}
}
void readMaster(){
slaveRadio.readData(&_radioData);
// Note how '&' must be placed in front of the variable name.
whichButton = _radioData.pushbuttonNumber;
//delay(500);
selectServo();
reportPointchange();
}
void selectServo(){
// button2servomap
for (int servomatch = 0; servomatch <10; servomatch++){
if (button2servomap[servomatch] == whichButton){
whichServo = servomatch;
break;
}
}
switch (whichServo){
case 0 ... 8: // points 8 to 16; double slip is 17/18
// get current position of that point
if ((currentPosition[whichServo])==1){
//move point to turnout
changePoint(2);
currentPosition[whichServo] = 2;
}else{
//move point to straight 1
changePoint(1);
currentPosition[whichServo] = 1;
}
// read point status
break;
case 9 ... 10: //double slip servos 9 & 10
// This slip requires different handling.Has two dependent servos
changedoubleSlip();
checkdoubleslipMicros();
break;
}
}
void changePoint(int whichWay){
switch (whichWay){
case 1:
Myservo.Servo(whichServo, servoHomePos[whichServo]); //servo back to home
delay(500); //allow time for servo to move
checkPoint(whichWay);
break;
case 2:
Myservo.Servo(whichServo, servoTurnoutPos[whichServo]); // set servo to turnout
delay(500);
checkPoint(whichWay);
break;
}
}
void checkPoint(int newPosition){
didpointChange = false;
straightMS = false;
turnoutMS = false;
switch (newPosition){
case 1:
straightMS = digitalRead(straightMSPin[whichServo]);
turnoutMS = digitalRead(turnoutMSPin[whichServo]);
if (straightMS== HIGH){; //straightMS closed
didpointChange = true;
currentPosition[whichServo]= 1;
Serial.print("Straight ");
Serial.println(straightMS);
};
break;
case 2:
straightMS =digitalRead(straightMSPin[whichServo]);
turnoutMS = digitalRead(turnoutMSPin[whichServo]);
if (turnoutMS== HIGH){;
didpointChange = true;
currentPosition[whichServo] = 2;
Serial.print("Turnout ");
Serial.println(turnoutMS);
}
break;
}
if (didpointChange){
Serial.print(whichServo);
Serial.println(" changed");
} else {
Serial.print(whichServo);
Serial.println(" not changed");
}
Serial.print("Straight Micropin ");
Serial.println(straightMSPin[whichServo]);
Serial.print("Turnout Micropin ");
Serial.println(turnoutMSPin[whichServo]);
Serial.print("straightMS value ");
Serial.println(straightMS);
Serial.print("turnoutMS value ");
Serial.println(turnoutMS);
}
void changedoubleSlip(){
// both servos are at home pos from setup.
switch (currentposdoubleSlip){
case 1:
//move servo DS1 to turnout position
Myservo.Servo(DS1, DS1servoTurnout);
currentposdoubleSlip++;
break;
case 2:
// move servo DS2 to turnout position
Myservo.Servo(DS2, DS2servoTurnout);
currentposdoubleSlip++;
break;
case 3:
// move servo DS1 to home position
Myservo.Servo(DS1, DS1servoHome);
currentposdoubleSlip++;
break;
case 4:
// move DS2 to home position
Myservo.Servo(DS2, DS2servoHome);
currentposdoubleSlip=1;
break;
}
currentpointPos - currentposdoubleSlip;
}
void checkdoubleslipMicros(){
didpointChange = false;
bool DSMSa = false;
bool DSMSb = false;
switch (currentposdoubleSlip){
case 1:
DSMSa = digitalRead(DS1straightPin); //40
DSMSb = digitalRead(DS2straightPin); //42
didpointChange = (DSMSa && DSMSb);
break;
case 2:
DSMSa = digitalRead(DS1turnoutPin); //41
DSMSb = digitalRead(DS2straightPin); //42
didpointChange = (DSMSa && DSMSb);
break;
case 3:
DSMSa = digitalRead(DS1turnoutPin); //41
DSMSb = digitalRead(DS2turnoutPin); //43
didpointChange = (DSMSa && DSMSb);
break;
case 4:
DSMSa = digitalRead(DS1straightPin); //40
DSMSb = digitalRead(DS2turnoutPin); //43
didpointChange = (DSMSa && DSMSb);
break;
}
}
void reportPointchange(){
_radioData.FromRadioId = 2;
_radioData.pushbuttonNumber = whichButton;
_radioData.didchange = didpointChange;
_radioData.pointStatus = currentpointPos;
if(slaveRadio.send(MasterradioID, &_radioData, sizeof(_radioData))){
Serial.print(_radioData.didchange);
Serial.println(" Data sent ok");
Serial.println();
}
}
void loop(){
while(!slaveRadio.hasData()){
}
readMaster();
}