DCC decoder - servo

<COMPONENTEN>

<BENODIGDE LIBRARIES>

https://github.com/mrrwa/NmraDcc

<CODE>

#include <NmraDcc.h>

#include <Servo.h>

#include <EEPROM.h>

#define maxaccessories 4

Servo servo[maxaccessories + 1];

#if defined(DCC_ACK_PIN)

const int DccAckPin = DCC_ACK_PIN ;

#endif

NmraDcc Dcc ;

DCC_MSG Packet ;

typedef struct {

int address;

byte output;

int servoPin;

int ledPin;

int relaisAPin;

int relaisBPin;

int positionA;

int positionB;

int speed;

int positionStart;

int currentPos;

int direction;

int positiontogo;

int startposition;

int sleep;

int ACKcount;

int ACKtime;

int ACKdirection;

int setServo;

}

DCCAccessoryAddress;

DCCAccessoryAddress accessory[maxaccessories + 1];

int addr = 0;

int ProgState = 0;

int ProgRun = 0;

int ProgLed = 18;

int ProgAddr = 0;

int ProgCounter = 0;

int ProgPosition = 0; //0 = positionA , 1 = positionB

int EEAddr = 0;

int servoInArray = 0;

int sevoSpeed = 0;

int stopWhile;

unsigned long time;

int ButtonLed = 19;

boolean pressedP;

int buttonP = 4; // p4

int buttonStateP;

int lastbuttonStateP = LOW;

long lastDebounceTimeP = 0;

long debounceDelayP = 70;

boolean pressedA;

int buttonA = 5; //p5

int buttonStateA;

int lastButtonStateA;

long lastDebounceTimeA;

long debounceDelayA = 100;

boolean pressedB;

int buttonB = 3; // p3

int buttonStateB;

int lastButtonStateB;

long lastDebounceTimeB;

long debounceDelayB = 100;

void(* resetFunc) (void) = 0; //declare reset function at address 0

boolean ProgramButtonDebounce() // read button status

{

boolean retVal = false;

int reading = digitalRead(buttonP);

if (reading != lastbuttonStateP) {

lastDebounceTimeP = millis();

}

if ((millis() - lastDebounceTimeP) > debounceDelayP) {

if (reading != buttonStateP) {

buttonStateP = reading;

if (buttonStateP == HIGH) {

retVal = true;

}

}

}

lastbuttonStateP = reading;

return retVal;

}

boolean DebounceA() // read button status

{

boolean retVal = false;

int reading = digitalRead(buttonA);

if (reading != lastButtonStateA) {

lastDebounceTimeA = millis();

}

if ((millis() - lastDebounceTimeA) > debounceDelayA) {

if (reading != buttonStateA) {

buttonStateA = reading;

if (buttonStateA == HIGH) {

retVal = true;

}

}

}

lastButtonStateA = reading;

return retVal;

}

boolean DebounceB() // read button status

{

boolean retVal = false;

int reading = digitalRead(buttonB);

if (reading != lastButtonStateB) {

lastDebounceTimeB = millis();

}

if ((millis() - lastDebounceTimeB) > debounceDelayB) {

if (reading != buttonStateB) {

buttonStateB = reading;

if (buttonStateB == HIGH) {

retVal = true;

}

}

}

lastButtonStateB = reading;

return retVal;

}

//Turnouts - may not be properly implemented in library, problem with BoardAddress. Must turn off my-address-only check. must be in FLAGS_DCC_ACCESSORY_DECODER mode.

extern void notifyDccAccState( uint16_t Addr, uint16_t BoardAddr, uint8_t OutputAddr, uint8_t State ) {

uint8_t OutputNum = OutputAddr >> 1; //shift over the bits so the outputaddr is 0 to 3

uint8_t StateProper = OutputAddr & 0b00000001; //JMRI puts out the state as the right most bit of pDccMsg->Data[1], the state argument doesnt change in JMRI Turnout.

if (ProgRun == 9)

{

Serial.println("Writing settings to EEPROM: address");

EEPROM.write(ProgAddr, highByte(Addr)); // for integers bigger than 254

EEPROM.write(ProgAddr + 1, lowByte(Addr)); // for integers bigger than 254

Serial.print("--> New first address attributed: ");

Serial.println(Addr);

delay(300);

digitalWrite(ProgLed, LOW);

delay(300);

Serial.println("Reset decoder");

delay(300);

resetFunc(); //call reset

}

// set directions

for (int i = 1; i < maxaccessories + 1; i++)

{

if (Addr == accessory[i].address)

{

if (StateProper == 1)

{

Serial.print("address enable-rechtdoor:");

Serial.println(Addr);

accessory[i].output = 1;

accessory[i].positiontogo = accessory[i].positionA;

accessory[i].positionStart = accessory[i].positionB;

}

else

{

Serial.print("address disable-afslaan:");

Serial.println(Addr);

accessory[i].output = 1;

accessory[i].positiontogo = accessory[i].positionB;

accessory[i].positionStart = accessory[i].positionA;

}

if (accessory[i].currentPos >= accessory[i].positiontogo)

{

accessory[i].direction = -1;

}

else {

accessory[i].direction = +1;

}

if (accessory[i].direction == accessory[i].ACKdirection)

{

accessory[i].ACKcount = accessory[i].ACKcount + 1;

}

else

{

accessory[i].ACKcount = 0;

accessory[i].ACKtime = 0;

accessory[i].ACKdirection = accessory[i].direction;

}

}// End Addr

}

delay(10);

}// End notifyDccAccState

void blinkProgLed(int i)

{

for (int d = 0; d < i; d++)

{

delay(200);

digitalWrite(ProgLed, LOW);

delay(200);

digitalWrite(ProgLed, HIGH);

}

}// End blinkProgLed

void blinkButtonLed()

{

digitalWrite(ButtonLed, HIGH);

delay(2);

digitalWrite(ButtonLed, LOW);

} //blinkButtonLed

void printServo(int i)

{

Serial.print("servo: ");

Serial.print(i);

Serial.print(" - posA: ");

Serial.print(accessory[i].positionA);

Serial.print(" - posB: ");

Serial.print(accessory[i].positionB);

Serial.print(" - curPos: ");

Serial.print(accessory[i].currentPos);

Serial.print(" - goto: ");

Serial.print(accessory[i].positiontogo);

Serial.print(" - dir: ");

Serial.print(accessory[i].direction);

Serial.print(" - speed: ");

Serial.print(accessory[i].speed);

Serial.print(" - ");

Serial.println(millis() - time);

time = millis();

}// End printServo

void setServoPosition(int i, int pos)

{

printServo(i);

servo[i].write(pos);

}// End setServoPosition

void setup() {

delay(10);

Serial.begin(115200);

delay(10);

Serial.println("Decoder started");

EEAddr = word(EEPROM.read(ProgAddr), EEPROM.read(ProgAddr + 1)); // for integers bigger than 254

Serial.print("Reading from EEPROM --> Address first output: ");

Serial.println(EEAddr, DEC);

for (int i = 1; i < maxaccessories + 1; i++)

{

accessory[i].positionA = EEPROM.read((ProgAddr + 2 + ((i - 1) * 4) + 0)); //position

accessory[i].positionB = EEPROM.read((ProgAddr + 2 + ((i - 1) * 4) + 1));

accessory[i].currentPos = EEPROM.read((ProgAddr + 2 + ((i - 1) * 4) + 2)); //currentposition

accessory[i].speed = EEPROM.read((ProgAddr + 2 + ((i - 1) * 4) + 3)); //speed

if (accessory[i].speed < 5 || accessory[i].speed > 100) {

accessory[i].speed = 20;

}

printServo(i);

}

Serial.println("End reading from EEPROM");

// Configure remaining Decoder Functions

for (int i = 1; i < maxaccessories + 1; i++)

{

accessory[i].address = EEAddr + i - 1; // DCC address

accessory[i].output = 0; // Initial state (0 or 1) of output after startup.

accessory[i].servoPin = 10 - i; // 9,8,7,6

//accessory[i].ledPin = 9 + i; // 10,11,12,13

accessory[i].relaisAPin = 8 + (2 * i);

accessory[i].relaisBPin = 8 + (2 * i) + 1;

accessory[i].currentPos = (accessory[i].positionA + accessory[i].positionB) / 2;

accessory[i].positiontogo = accessory[i].positionA;

accessory[i].sleep = 0;

accessory[i].setServo = 0;

if (accessory[i].currentPos >= accessory[i].positionA) {

accessory[i].direction = -1;

accessory[i].ACKdirection = -1;

}

else {

accessory[i].direction = +1;

accessory[i].ACKdirection = 1;

}

pinMode(accessory[i].servoPin, OUTPUT);

pinMode(accessory[i].relaisAPin, OUTPUT);

pinMode(accessory[i].relaisBPin, OUTPUT);

//pinMode(accessory[i].ledPin, OUTPUT);

//Serial.println(accessory[i].relaisAPin);

printServo(i);

}

Serial.println("------------------------");

// set all servos to start position

for (int i = 1; i < maxaccessories + 1; i++)

{

servo[i].attach(accessory[i].servoPin);

printServo(i);

delay(100);

for (int j = accessory[i].currentPos; j != accessory[i].positiontogo; j = j + accessory[i].direction)

{

setServoPosition(i, j);

delay(20);

}

delay(100);

servo[i].detach();

delay(100);

accessory[i].currentPos = accessory[i].positiontogo;

}

// switch Relais A on

for (int i = 1; i < maxaccessories + 1; i++)

{

digitalWrite(accessory[i].relaisBPin, LOW);

delay(100);

digitalWrite(accessory[i].relaisAPin, HIGH);

}

// leds & buttons

pinMode(ProgLed, OUTPUT);

pinMode(ButtonLed, OUTPUT);

pinMode(buttonP, INPUT);

pinMode(buttonA, INPUT);

pinMode(buttonB, INPUT);

#if defined(DCCACKPIN)

//Setup ACK Pin

pinMode(DccAckPin, OUTPUT);

digitalWrite(DccAckPin, 0);

#endif

// Setup which External Interrupt, the Pin it's associated with that we're using, disable pullup.

Dcc.pin(0, 2, 0);

// Call the main DCC Init function to enable the DCC Receiver

Dcc.init( MAN_ID_DIY, 100, FLAGS_DCC_ACCESSORY_DECODER , 0 );

Serial.println("Decoder setup Ready");

}// End setup

void loop() {

Dcc.process();

//Lets check the program Button

pressedP = ProgramButtonDebounce(); // buttonP

if (pressedP == true) {

blinkButtonLed();

Serial.print("Programming button pressed - ");

ProgState = 1;

ProgRun = 1;

ProgCounter++;

Serial.println(ProgCounter);

}

if (ProgState == 1)

{

pressedA = DebounceA(); // buttonA

pressedB = DebounceB(); // buttonB

}

if (ProgState == 1)

{

if (ProgRun == 1)

{

switch (ProgCounter) { // number of times the programming button was pressed

case 1: // Address

Serial.print("Programming mode - decoder - begin address: ");

Serial.println(accessory[0].address);

ProgRun = 9; // address setting

digitalWrite(ProgLed, HIGH);

break;

case 2: // servo 1 - A

servoInArray = 1;

Serial.print("Programming mode - servo 1 - position A: ");

Serial.println(accessory[servoInArray].positionA);

ProgRun = 2; // servo setting

ProgPosition = 0; //PositionA

servo[servoInArray].attach(accessory[servoInArray].servoPin); // swith on this servo

digitalWrite(accessory[servoInArray].ledPin, HIGH); // swith on led

blinkProgLed(servoInArray);

delay(10);

setServoPosition(servoInArray, accessory[servoInArray].positionA);

break;

case 3: // servo 1 - B

servoInArray = 1;

Serial.print("Programming mode - servo 1 - position B: ");

Serial.println(accessory[servoInArray].positionB);

ProgRun = 2;

ProgPosition = 1; //PositionB

blinkProgLed(servoInArray);

setServoPosition(servoInArray, accessory[servoInArray].positionB);

break;

case 4: // servo 1 - speed

servoInArray = 1;

Serial.print("Programming mode - servo 1 - speed: ");

Serial.println(accessory[servoInArray].speed);

ProgRun = 3; // servo speed

stopWhile = 0;

accessory[servoInArray].sleep = 0;

if (accessory[servoInArray].positionA < accessory[servoInArray].positionB )

{

accessory[servoInArray].startposition = accessory[servoInArray].positionA;

accessory[servoInArray].positiontogo = accessory[servoInArray].positionB;

}

else

{

accessory[servoInArray].startposition = accessory[servoInArray].positionB;

accessory[servoInArray].positiontogo = accessory[servoInArray].positionA;

}

blinkProgLed(servoInArray);

break;

case 5: // servo 2 - A

servoInArray = 2;

Serial.print("Programming mode - servo 2 - position A: ");

Serial.println(accessory[servoInArray].positionA);

ProgRun = 2;

ProgPosition = 0; //PositionA

servo[servoInArray - 1].detach(); // switch off previous servo

digitalWrite(accessory[servoInArray - 1].ledPin, LOW); // swith off led

delay(10);

servo[servoInArray].attach(accessory[servoInArray].servoPin); // swith on this servo

digitalWrite(accessory[servoInArray].ledPin, HIGH); // swith on led

blinkProgLed(servoInArray);

delay(10);

setServoPosition(servoInArray, accessory[servoInArray].positionA);

break;

case 6: // servo 2 - B

servoInArray = 2;

Serial.print("Programming mode - servo 2 - position B: ");

Serial.println(accessory[servoInArray].positionB);

ProgRun = 2;

ProgPosition = 1; //PositionB

blinkProgLed(servoInArray);

setServoPosition(servoInArray, accessory[servoInArray].positionB);

break;

case 7: // servo 2 - speed

servoInArray = 2;

Serial.print("Programming mode - servo 2 - speed: ");

Serial.println(accessory[servoInArray].speed);

ProgRun = 3; // servo speed

stopWhile = 0;

accessory[servoInArray].sleep = 0;

if (accessory[servoInArray].positionA < accessory[servoInArray].positionB )

{

accessory[servoInArray].startposition = accessory[servoInArray].positionA;

accessory[servoInArray].positiontogo = accessory[servoInArray].positionB;

}

else

{

accessory[servoInArray].startposition = accessory[servoInArray].positionB;

accessory[servoInArray].positiontogo = accessory[servoInArray].positionA;

}

blinkProgLed(servoInArray);

break;

case 8: // servo 3 - A

servoInArray = 3;

Serial.print("Programming mode - servo 3 - position A: ");

Serial.println(accessory[servoInArray].positionA);

ProgRun = 2;

ProgPosition = 0; //PositionA

servo[servoInArray - 1].detach(); // switch off previous servo

digitalWrite(accessory[servoInArray - 1].ledPin, LOW); // swith off led

delay(10);

servo[servoInArray].attach(accessory[servoInArray].servoPin); // swith on this servo

digitalWrite(accessory[servoInArray].ledPin, HIGH); // swith on led

blinkProgLed(servoInArray);

delay(10);

setServoPosition(servoInArray, accessory[servoInArray].positionA);

break;

case 9: // servo 3 - B

servoInArray = 3;

Serial.print("Programming mode - servo 3 - position B: ");

Serial.println(accessory[servoInArray].positionB);

ProgRun = 2;

ProgPosition = 1; //PositionB

blinkProgLed(servoInArray);

setServoPosition(servoInArray, accessory[servoInArray].positionB);

break;

case 10: // servo 3 - speed

servoInArray = 3;

Serial.print("Programming mode - servo 3 - speed: ");

Serial.println(accessory[servoInArray].speed);

ProgRun = 3; // servo speed

stopWhile = 0;

accessory[servoInArray].sleep = 0;

if (accessory[servoInArray].positionA < accessory[servoInArray].positionB )

{

accessory[servoInArray].startposition = accessory[servoInArray].positionA;

accessory[servoInArray].positiontogo = accessory[servoInArray].positionB;

}

else

{

accessory[servoInArray].startposition = accessory[servoInArray].positionB;

accessory[servoInArray].positiontogo = accessory[servoInArray].positionA;

}

blinkProgLed(servoInArray);

break;

case 11: // servo 4 - A

servoInArray = 4;

Serial.print("Programming mode - servo 4 - position A: ");

Serial.println(accessory[servoInArray].positionA);

ProgRun = 2;

ProgPosition = 0; //PositionA

servo[servoInArray - 1].detach(); // switch off previous servo

digitalWrite(accessory[servoInArray - 1].ledPin, LOW); // swith off led

delay(10);

servo[servoInArray].attach(accessory[servoInArray].servoPin); // swith on this servo

digitalWrite(accessory[servoInArray].ledPin, HIGH); // swith on led

blinkProgLed(servoInArray);

delay(10);

setServoPosition(servoInArray, accessory[servoInArray].positionA);

break;

case 12: // servo 4 - B

servoInArray = 4;

Serial.print("Programming mode - servo 4 - position B: ");

Serial.println(accessory[servoInArray].positionB);

ProgRun = 2;

ProgPosition = 1; //PositionB

blinkProgLed(servoInArray);

setServoPosition(servoInArray, accessory[servoInArray].positionB);

break;

case 13: // servo 4 - speed

servoInArray = 4;

Serial.print("Programming mode - servo 4 - speed: ");

Serial.println(accessory[servoInArray].speed);

ProgRun = 3; // servo speed

stopWhile = 0;

accessory[servoInArray].sleep = 0;

if (accessory[servoInArray].positionA < accessory[servoInArray].positionB )

{

accessory[servoInArray].startposition = accessory[servoInArray].positionA;

accessory[servoInArray].positiontogo = accessory[servoInArray].positionB;

}

else

{

accessory[servoInArray].startposition = accessory[servoInArray].positionB;

accessory[servoInArray].positiontogo = accessory[servoInArray].positionA;

}

blinkProgLed(servoInArray);

break;

default: // End of decoder programming

Serial.println("End Programming mode");

Serial.println("Writing settings to EEPROM: positionA & positionB");

ProgState = 0;

ProgRun = 0;

ProgCounter++;

servo[servoInArray].detach(); // switch off previous servo // servoInArray is still 4

digitalWrite(accessory[servoInArray].ledPin, LOW); // swith off led

delay(10);

for (int i = 1; i < maxaccessories + 1; i++)

{

printServo(i);

EEPROM.write((ProgAddr + 2 + ((i - 1) * 4) + 0), accessory[i].positionA); //positionA

EEPROM.write((ProgAddr + 2 + ((i - 1) * 4) + 1), accessory[i].positionB); //positionB

EEPROM.write((ProgAddr + 2 + ((i - 1) * 4) + 2), accessory[i].currentPos); //currentposition

EEPROM.write((ProgAddr + 2 + ((i - 1) * 4) + 3), accessory[i].speed); //speed

}

delay(200);

digitalWrite(ProgLed, LOW);

delay(300);

Serial.println("Reset decoder");

delay(200);

resetFunc(); //call reset

}// End Switch

}// End ProgRun == 1

if (ProgRun == 2)

{

// buttonA

if (pressedA == true) {

blinkButtonLed();

Serial.print("Button A pressed - ");

if (ProgPosition == 0) //PositionA ?

{

accessory[servoInArray].positionA = accessory[servoInArray].positionA + 1;

if (accessory[servoInArray].positionA < 0) {

accessory[servoInArray].positionA = 0;

}

if (accessory[servoInArray].positionA > 180) {

accessory[servoInArray].positionA = 180;

}

setServoPosition(servoInArray, accessory[servoInArray].positionA);

Serial.print(servoInArray);

Serial.print(" - ");

Serial.println(accessory[servoInArray].positionA);

}

else //PositionB

{

accessory[servoInArray].positionB = accessory[servoInArray].positionB + 1;

if (accessory[servoInArray].positionB < 0) {

accessory[servoInArray].positionB = 0;

}

if (accessory[servoInArray].positionB > 180) {

accessory[servoInArray].positionB = 180;

}

setServoPosition(servoInArray, accessory[servoInArray].positionB);

Serial.print(servoInArray);

Serial.print(" - ");

Serial.println(accessory[servoInArray].positionB);

}

} // End ButtonA

//buttonB

if (pressedB == true) {

blinkButtonLed();

Serial.print("Button B pressed - ");

if (ProgPosition == 0) //PositionA

{

accessory[servoInArray].positionA = accessory[servoInArray].positionA - 1;

if (accessory[servoInArray].positionA < 0) {

accessory[servoInArray].positionA = 0;

}

if (accessory[servoInArray].positionA > 180) {

accessory[servoInArray].positionA = 180;

}

setServoPosition(servoInArray, accessory[servoInArray].positionA);

Serial.print(servoInArray);

Serial.print(" - ");

Serial.println(accessory[servoInArray].positionA);

}

else //PositionB

{

accessory[servoInArray].positionB = accessory[servoInArray].positionB - 1;

if (accessory[servoInArray].positionB < 0) {

accessory[servoInArray].positionB = 0;

}

if (accessory[servoInArray].positionB > 180) {

accessory[servoInArray].positionB = 180;

}

setServoPosition(servoInArray, accessory[servoInArray].positionB);

Serial.print(servoInArray);

Serial.print(" - ");

Serial.println(accessory[servoInArray].positionB);

}

}// End ButtonB

}// End ProgRun == 1

if (ProgRun == 3) // servo speed during setup

{

servo[servoInArray].attach(accessory[servoInArray].servoPin);

delay(50);

//stopWhile = 0;

while (stopWhile < 1)

{

for (accessory[servoInArray].currentPos = accessory[servoInArray].startposition; accessory[servoInArray].currentPos < accessory[servoInArray].positiontogo; accessory[servoInArray].currentPos++)

{

setServoPosition(servoInArray, accessory[servoInArray].currentPos);

delay(accessory[servoInArray].speed);

pressedA = DebounceA(); // buttonA

pressedB = DebounceB(); // buttonB

pressedP = ProgramButtonDebounce(); // buttonP

// buttonA

if (pressedA == true) {

blinkButtonLed();

Serial.print("Button A pressed - ");

accessory[servoInArray].speed = accessory[servoInArray].speed + 1;

Serial.print("speed - ");

Serial.println(accessory[servoInArray].speed);

} // End ButtonA

//buttonB

if (pressedB == true) {

blinkButtonLed();

Serial.print("Button B pressed - ");

accessory[servoInArray].speed = accessory[servoInArray].speed - 1;

Serial.print("speed - ");

Serial.println(accessory[servoInArray].speed);

}// End ButtonB

if (pressedP == true) {

blinkButtonLed();

Serial.println("Stop pressed");

stopWhile = 1;

//break;

}

}

for (accessory[servoInArray].currentPos = accessory[servoInArray].positiontogo; accessory[servoInArray].currentPos > accessory[servoInArray].startposition; accessory[servoInArray].currentPos--)

{

setServoPosition(servoInArray, accessory[servoInArray].currentPos);

delay(accessory[servoInArray].speed);

pressedA = DebounceA(); // buttonA

pressedB = DebounceB(); // buttonB

pressedP = ProgramButtonDebounce(); // buttonP

// buttonA

if (pressedA == true) {

blinkButtonLed();

;

Serial.print("Button A pressed - ");

accessory[servoInArray].speed = accessory[servoInArray].speed + 1;

Serial.print("speed - ");

Serial.println(accessory[servoInArray].speed);

} // End ButtonA

//buttonB

if (pressedB == true) {

blinkButtonLed();

Serial.print("Button B pressed - ");

accessory[servoInArray].speed = accessory[servoInArray].speed - 1;

Serial.print("speed - ");

Serial.println(accessory[servoInArray].speed);

}// End ButtonB

if (pressedP == true) {

blinkButtonLed();

Serial.println("Stop pressed");

stopWhile = 1;

//break;

}

}

if (accessory[servoInArray].speed < 0) {

accessory[servoInArray].speed = 0;

}

if (accessory[servoInArray].speed > 120) {

accessory[servoInArray].speed = 120;

}

} // end wile

servo[servoInArray].detach();

delay(30);

}// End ProgRun == 3

}// End ProgState == 1

for (int i = 1; i < maxaccessories + 1; i++)

{

if (accessory[i].ACKcount > 0)

{

if (accessory[i].ACKtime > 60)

{

ProgRun = 4;

Serial.print(" ACK 4: ");Serial.print(i);

accessory[i].setServo = 1;

accessory[i].ACKcount = 0;

accessory[i].ACKtime = 0;

}

accessory[i].ACKtime = accessory[i].ACKtime + 1;

Serial.print(" ACK : ");Serial.print(i);

Serial.print(" ACKcount : ");Serial.print(accessory[i].ACKcount);

Serial.print(" ACKtime : ");Serial.println(accessory[i].ACKtime);

}

} // End for

// action on servo settings

//if (ProgState != 1)

if (ProgRun != 2 && ProgRun != 3) // 10/10/2015

{

for (int i = 1; i < maxaccessories + 1; i++)

{

//if (accessory[i].output == 1)

// if ((accessory[i].positionStart == accessory[i].positionA && accessory[i].positiontogo == accessory[i].positionB)

// || (accessory[i].positionStart == accessory[i].positionB && accessory[i].positiontogo == accessory[i].positionA))

if (accessory[i].setServo > 0)

{

//if (not servo[i].attached())

//if ((accessory[i].currentPos == accessory[i].positionA || accessory[i].currentPos == accessory[i].positionB) &&(accessory[i].currentPos != accessory[i].positiontogo))

if (accessory[i].setServo == 1 && accessory[i].currentPos != accessory[i].positiontogo)

{

Serial.print("Attach servo : ");Serial.print(i);

Serial.print(" A : "); Serial.print(accessory[i].positionA);

Serial.print(" B : "); Serial.print(accessory[i].positionB);

Serial.print(" start : "); Serial.print(accessory[i].positionStart);

Serial.print(" curr : "); Serial.print(accessory[i].currentPos);

Serial.print(" to go : "); Serial.print(accessory[i].positiontogo);

Serial.print(" direc : "); Serial.println(accessory[i].direction);

servo[i].attach(accessory[i].servoPin);

//digitalWrite(accessory[i].ledPin, HIGH);

digitalWrite(accessory[i].relaisAPin, LOW);

digitalWrite(accessory[i].relaisBPin, LOW);

accessory[i].setServo = accessory[i].setServo + 1;

delay(50);

}

if (accessory[i].currentPos != accessory[i].positiontogo && accessory[i].sleep >= accessory[i].speed)

{

accessory[i].currentPos = accessory[i].currentPos + accessory[i].direction;

accessory[i].sleep = 0;

setServoPosition(i, accessory[i].currentPos);

accessory[i].sleep = 0;

}

if (accessory[i].setServo == 2 && accessory[i].currentPos == accessory[i].positiontogo) // end of move

{

accessory[i].positionStart = 0;

Serial.print("Detach servo : ");Serial.print(i);

Serial.print(" A : "); Serial.print(accessory[i].positionA);

Serial.print(" B : "); Serial.print(accessory[i].positionB);

Serial.print(" start : "); Serial.print(accessory[i].positionStart);

Serial.print(" curr : "); Serial.print(accessory[i].currentPos);

Serial.print(" to go : "); Serial.print(accessory[i].positiontogo);

Serial.print(" direc : "); Serial.println(accessory[i].direction);

accessory[i].output = 0;

delay(20);

servo[i].detach();

accessory[i].setServo = 0;

//digitalWrite(accessory[i].ledPin, LOW);

if (accessory[i].direction == 1)

{

digitalWrite(accessory[i].relaisAPin, HIGH);

}

else

{

digitalWrite(accessory[i].relaisBPin, HIGH);

}

delay(50);

}

accessory[i].sleep = accessory[i].sleep + 1;

delay(1);

//printServo(i);

} // End if

} // End for

} // End if

}// End loop

/*

progmode

- address set

- servo 1 (led blinks 1 time)

- position A

- position B

- speed

- servo 2 (led blinks 2 times)

- position A

- position B

- speed

- servo 3 (led blinks 3 times)

- position A

- position B

- speed

- servo 4 (led blinks 4 times)

- position A

- position B

- speed

- write to EEPROM

- reset

*/

/* EEPROM setup

0 - highByte(Addr)

1 - lowByte(Addr)

2 - servo 1 - positionA

3 - servo 1 - positionB

4 - servo 1 - current position

5 - servo 1 - speed

6 - servo 2 - positionA

7 - servo 2 - positionB

8 - servo 1 - current position

9 - servo 2 - speed

10 - servo 3 - positionA

11 - servo 3 - positionB

12 - servo 1 - current position

13 - servo 3 - speed

14 - servo 4 - positionA

15 - servo 4 - positionB

16 - servo 1 - current position

17 - servo 4 - speed

*/

/* pin setup voor versie met relais

pin 2 : DCC signal

pin 3 : buttonA

pin 4 : buttonP

pin 5 : buttonB

pin 6 : servo 4

pin 7 : servo 3

pin 8 : servo 2

pin 9 : servo 1

pin 10 : relais A of servo 4

pin 11 : relais B of servo 4

pin 12 : relais A of servo 3

pin 13 : relais B of servo 3

pin A0 : relais A of servo 2

pin A1 : relais B of servo 2

pin A2 : relais A of servo 1

pin A3 : relais B of servo 1

pin A4 : led servo

pin A5 : led button

*/

/*

a digital servo (sg92r) works better than an analog one (sg90)

as the analog one return to the midpoint when starting up and the digital one does not, so to avoid jitter at startup

*/

<>

Valentijn 25-10-2015

October 2015