//// A function that executes whenever a DCC_ACC message is received from the Canbus// It's purpose is to analyze the received event (Signal or turnout) and// update the track position of the TT or update the position of the doors.//bool calibrated = false;void dccAccReceiver(unsigned char aMsgLen, DCC_ACC_MSG *msgData){#if (DEBUG) Serial.print("[dccAccReceiver] dccId: "); Serial.print(msgData->address); Serial.print(", dccState: "); Serial.println(msgData->direction); Serial.flush();#endif //DEBUG unsigned short address = msgData->address; // // Do we perform a straight(-) or a diverging(/) operation? // For a straight operation enable is true, for a diverging operation // enable is false. // bool enable = (msgData->direction) ? 0 : 1; // // Search for the specified accessory address, when found lookup the // specified position for the straight or diverging operation and start moving // to that target position. // for (int i = 0; i < nrOfTracks; i++) { if ( address == trackPositions[i].address ) {#if (DEBUG) Serial.print("[dccAccReceiver] TT Address: "); Serial.print(address, DEC);#endif //DEBUG // // The new position of the TT // long newTargetPosition = 0; if (enable) // Received a "-" command {#if (DEBUG) Serial.print(" (-)");#endif //DEBUG // // Go to the straight position (0 degree) // newTargetPosition = trackPositions[i].position0; // // If the new target address equals zero, a calibration is asked for // In that case we don't save the new position, because the current // position stay active after the calibration. // if (newTargetPosition != 0) { EepromControl->setTTposition(i); EepromControl->saveData(); } } else // Received a "/" command {#if (DEBUG) Serial.print(" (/)");#endif //DEBUG // Go to the diverging (180 degree) position. // newTargetPosition = trackPositions[i].position180; // // If the new target address equals zero, a calibration is asked for // In that case we don't save the new position, because the current // position stay active after the calibration. // if (newTargetPosition != 0) { EepromControl->setTTposition(i+nrOfTracks); EepromControl->saveData(); } }#if (DEBUG) Serial.print(" Position: "); Serial.println(newTargetPosition, DEC); Serial.flush();#endif //DEBUG // // If the choosen position equals 0, then we have to recalibrate // After the calibration we return to the current position, so we have to // get that from Eeprom. Further after a calibration, we block new calibrations // until a regular movement is executed. // if ((newTargetPosition == 0) && (!calibrated)) { // // Load the EEPROM data for the turnout directions // EepromControl->loadData(); byte eePromData = EepromControl->getTTposition(); // // Savety measure, for when the Eeprom contains garbage // if (eePromData > 2*nrOfTracks) { eePromData = 1; } // // Get the in EEPROM stored track position, it wil be used after calibration // of the turntable to return to the current position // if (eePromData < nrOfTracks) { TT.calibrate(trackPositions[eePromData].position0); } else { TT.calibrate(trackPositions[eePromData-nrOfTracks].position180); } calibrated = true; } else if (newTargetPosition != 0) { // // Not a recalibrate, goto the choosen position // TT.gotoTTPosition(newTargetPosition); // // Free to recalibrate // calibrated = false; } return; } } // // If we didn't find an address match with a track location // check if a door servo matches the received address // for (int i = 0; i < NR_OF_DOORS; i++) { if ( address == doors[i].address ) {#if (DEBUG) Serial.print("[dccAccReceiver] Door address: "); Serial.print(address, DEC);#endif //DEBUG if (enable) {#if (DEBUG) Serial.println(" (-)"); Serial.flush();#endif //DEBUG // // Go to the closed position // doors[i].doorServo->closeDoor(); EepromControl->setDoorPosition(i,DoorClosed); EepromControl->saveData(); } else {#if (DEBUG) Serial.println(" (/)"); Serial.flush();#endif //DEBUG // // Go to the open position. // doors[i].doorServo->openDoor(); EepromControl->setDoorPosition(i,DoorOpen); EepromControl->saveData(); } return; } }#if (DEBUG) Serial.print("[dccAccReceiver] Unknown address: "); Serial.println(address, DEC); Serial.flush();#endif //DEBUG}
//// The occupance detectors for the switches start on bank 4, each bank has 16 occupancies, so a single// bank is enough for the 12 turnouts in use.// The push buttons used for controlling these switches generate Occupancy detector events // starting on bank 6//#define S88_SWITCH_BANK 4#define S88_SWITCHBUTTON_BANK 6
//// Table definitions for the turnout/switch leds//const int nrOfTurnouts = 12;typedef struct { int s88Bank; int s88Nr; int pinStraight; int pinDiverting;} tolTable;tolTable turnoutLed[nrOfTurnouts] = { { S88_SWITCH_BANK, 1, 22, 23}, { S88_SWITCH_BANK, 2, 25, 24}, { S88_SWITCH_BANK, 3, 26, 27}, { S88_SWITCH_BANK, 4, 29, 28}, { S88_SWITCH_BANK, 5, 30, 31}, { S88_SWITCH_BANK, 6, 33, 32}, { S88_SWITCH_BANK, 7, 35, 34}, { S88_SWITCH_BANK, 8, 36, 37}, { S88_SWITCH_BANK, 9, 39, 38}, { S88_SWITCH_BANK,10, 40, 41}, { S88_SWITCH_BANK,11, 43, 42}, { S88_SWITCH_BANK,12, 44, 45} };
// // Setup and start the CanBus communication // canBus = new CanBus(PANEL_CONTROLLER_ID, new Mcp2515(49), new Mcp2515(53)); // Sender + Receiver canBus->setMsgReceiver(OCC_DT, occDtReceiver); canBus->setMsgReceiver(DCC_ACC, dccAccReceiver); canBus->begin();
// // Activate the turnout leds // for (int i = 0; i < nrOfTurnouts; i++) { // // Switch both leds off // pinMode(turnoutLed[i].pinStraight,OUTPUT); digitalWrite(turnoutLed[i].pinStraight, LOW); pinMode(turnoutLed[i].pinDiverting, OUTPUT); digitalWrite(turnoutLed[i].pinDiverting, LOW); }
void occDtReceiver(unsigned char aMsgLen, OCC_DT_MSG *msgData){ // // Find the proper turnout detector with the matching address // and sets its new direction based on the enable field from the // received dcc command // for (int i = 0; i < nrOfTurnouts; i++) { if ((turnoutLed[i].s88Nr+((turnoutLed[i].s88Bank - 1) * 16)-1) == msgData->address) { // // Switch both leds off // digitalWrite(turnoutLed[i].pinStraight, LOW); digitalWrite(turnoutLed[i].pinDiverting, LOW); if (msgData->state == 0) { // // Set the straight led // digitalWrite(turnoutLed[i].pinStraight, HIGH); } else { // // Set the diverging direction // digitalWrite(turnoutLed[i].pinDiverting, HIGH); } // // End the for loop // return; } }}
//// Class for a push button that controls a switch by generating an // occupance detector event when it is pushed.//class SwitchButton { private: bool state = false; // initial the button is not pushed int buttonPin; bool pushed = false; bool stateSet = false; bool bounceState = false; unsigned long bounceTime; // Last time a state switch happened int bounceWait = 4 ; // Ignore bounce for 4mS public: // // Create a new instance of a SwitchButton // SwitchButton(int aButtonPin); bool getState(); // // Attach the digital pin to the push button // void attach(); // // Check the state of the buttonPin. If it stays long enough // send out an occupance detector event. // void heartBeat();};
//// Create a new instance of a SwitchButton//SwitchButton::SwitchButton(int aButtonPin){ buttonPin = aButtonPin; pushed = false; // If pushed, it asks for diverging. state = false; bounceState = false;}//// Return the current state of the SwitchButton//bool SwitchButton::getState(){ return state;}//// Attach the digital pin to the push button//void SwitchButton::attach(){ pinMode(buttonPin, INPUT_PULLUP);}//// Check the state of the buttonPin. If it stays long enough// send out an occupance detector event.//void SwitchButton::heartBeat(){ if (!pushed) { // // The button state is not pushed, so wait until the pin goes HIGH // signaling a pushed button. // if (digitalRead(buttonPin) == HIGH) { // // Wait a small amount of time, this to make sure that the // state of the pin is constant // if (!bounceState) { bounceState = true; bounceTime = millis(); } else if (((millis() - bounceTime) > bounceWait)) { pushed = true; // Flip the state bounceState = false; // Reset the bounce state for the next time } } } else { // // The button state is pushed, so wait until the pin goes LOW // signaling a released button. // if (digitalRead(buttonPin) == LOW) { // // Wait a small amount of time, this to make sure that the // state of the pin is constant // if (!bounceState) { bounceState = true; bounceTime = millis(); } else if (((millis() - bounceTime) > bounceWait)) { pushed = false; // Reset the pushed state stateSet = false; bounceState = false; // Reset the bounce state for the next time } } } // // When the button was pushed and this is the first time we detect that, // flip the state // if ((pushed) && (!stateSet)) { stateSet = true; // The state must be set once, so this prevent // that their is a constant flipping of the state // // Flip the state // if (state == true) { state = false; } else { state = true; } }}
//// Table containing all definitions for pushbuttons on the panel//const int nrOfButtons = 10;typedef struct { SwitchButton button; bool currentState; int s88Bank; int s88Nr;} switchTable;switchTable buttonTable[nrOfButtons] = { {SwitchButton(A2), false, S88_SWITCHBUTTON_BANK, 1}, // Button for "Handmatige bezet melding" {SwitchButton(A3), false, S88_SWITCHBUTTON_BANK, 2}, // Button for "Handmatige bezet melding" {SwitchButton(A4), false, S88_SWITCHBUTTON_BANK, 3}, // Button for "Handmatige bezet melding" {SwitchButton(A5), false, S88_SWITCHBUTTON_BANK, 4}, // Button for "Omzetten wissel 4" {SwitchButton(A6), false, S88_SWITCHBUTTON_BANK, 5}, // Button for "Omzetten wissel 5" {SwitchButton(A7), false, S88_SWITCHBUTTON_BANK, 6}, // Button for "Omzetten wissel 6" {SwitchButton(A8), false, S88_SWITCHBUTTON_BANK, 7}, // Button for "Omzetten wissel 7" {SwitchButton(A9), false, S88_SWITCHBUTTON_BANK, 8}, // Button for "Omzetten wissel 8" {SwitchButton(A10),false, S88_SWITCHBUTTON_BANK, 9}, // Button for "Omzetten wissel 9" {SwitchButton(A11),false, S88_SWITCHBUTTON_BANK, 10} // Button for "Handmatige bezet melding"};
// // Activate the turnout push buttons // for (int i = 0; i < nrOfTurnouts; i++) { (buttonTable[i].button).attach(); }
// // Check the turnout push buttons, and if they were pushed, send an occupancy event // for (int i = 0; i < nrOfButtons; i++) { (buttonTable[i].button).heartBeat(); // The buttons have to monitor their pins // // If the state of the button has changed (it differs from the last stored state) // put an occupance message on the Canbus // if (buttonTable[i].currentState != (buttonTable[i].button).getState()) { // // The state has changed, so store the new state // buttonTable[i].currentState = (buttonTable[i].button).getState(); if ((buttonTable[i].button).getState() == true) { occDtHandler(buttonTable[i].s88Bank, buttonTable[i].s88Nr, true); } else { occDtHandler(buttonTable[i].s88Bank, buttonTable[i].s88Nr, false); } } }
void occDtHandler(int aS88Bank, int aS88BankNr, bool aDirection){ // // Send the new state to the S88 Interface // OCC_DT_MSG buf; // // At this moment we have no more the 12 turnouts, so don't bother about the S88 bank // buf.address = ((aS88Bank - 1) * 16) + (aS88BankNr - 1); if (aDirection) buf.state = 1; else buf.state = 0; canBus->sendMsg(OCC_DT, sizeof(OCC_DT_MSG), &buf);}
//// Definitions for the table with block leds. Leds that wil signal when a block is // reserved and may be used for switching operations.//const int nrOfBlokLeds = 4;typedef struct { int dccId; int ledPin;} BlokLed;BlokLed blokLeds[nrOfBlokLeds] = { {101, A12}, {102, A13}, {103, A14}, {110, A15}};
// // Activate the blok leds // for (int i = 0; i < nrOfBlokLeds; i++) { // // Switch led off // pinMode(blokLeds[i].ledPin,OUTPUT); digitalWrite(blokLeds[i].ledPin, LOW); }
//// A function that executes whenever a message is received from the Canbus// It's purpose is to analyze the received event and set the led for this address//void dccAccReceiver(unsigned char aMsgLen, DCC_ACC_MSG *msgData){ unsigned short dccId = msgData->address; byte dccState = msgData->direction; // // Find the led with the received address // for (int i=0; i<nrOfBlokLeds; i++) { if (blokLeds[i].dccId == dccId) { if (dccState == 0) { digitalWrite(blokLeds[i].ledPin, LOW); } else { digitalWrite(blokLeds[i].ledPin, HIGH); } return; } }}
class TTPositionSwitch { enum SWITCHSTATE { PIN_HIGH = 1, GOING_HIGH = 2, PIN_LOW = 3, GOING_LOW = 4 }; private: int ttPin; bool reverseDirection; SWITCHSTATE switchState; bool state; bool bounceState = false; unsigned long bounceTime; // Last time a Pin state switched, used to handle bounce int bounceWait = 1000 ; // Let the state changes settle for for 1 sec public: TTPositionSwitch(int aTTPin, bool aReverseDirection); bool getReverseDirection(); bool getState(); // // Attach the digital pin to the push button // void attach(); // // Check the state of the buttonPin. If it stays long enough // send out an occupance detector event. // void heartBeat();};
TTPositionSwitch::TTPositionSwitch(int aTTPin, bool aReverseDirection){ ttPin = aTTPin; reverseDirection = aReverseDirection; switchState = PIN_HIGH; state = false;}bool TTPositionSwitch::getReverseDirection(){ return reverseDirection;}//// Return it's current state//bool TTPositionSwitch::getState(){ return state;}
//// Attach the digital pin to the push button//void TTPositionSwitch::attach(){ pinMode(ttPin, INPUT_PULLUP); // // Initialize the current state of the switch. Don't wait for bouncing, // because it is save to asume that the switch is currently in a // stable position. // if (digitalRead(ttPin) == LOW) { state = true; switchState = PIN_LOW; } else { state = false; switchState = PIN_HIGH; }}
//// Check the state of the buttonPin. If it stays long enough// send out an occupance detector event.//void TTPositionSwitch::heartBeat(){ // // The switch is a rotating switch, so always one position is engaged, // so only when a pin goes HIGH we have to do something, send a DCC_ACC message // switch (switchState) { case PIN_HIGH: if (digitalRead(ttPin) == LOW) { switchState = GOING_LOW; bounceTime = millis(); } break; case GOING_HIGH: if ((millis() - bounceTime) > bounceWait) { if (digitalRead(ttPin) == HIGH) { state = false; switchState = PIN_HIGH; } else { switchState = PIN_LOW; } } break; case PIN_LOW: if (digitalRead(ttPin) == HIGH) { switchState = GOING_HIGH; bounceTime = millis(); } break; case GOING_LOW: if ((millis() - bounceTime) > bounceWait) { if (digitalRead(ttPin) == LOW) { // // If the state switches from LOW to HIGH, report it // state = true; switchState = PIN_LOW; } else { // // After the bounce time, the state of the pin has returned to HIGH // so reset the ttSwitchState // switchState = PIN_HIGH; } } break; }}
//// Table containg all definitions for the rotary turntable switch//const int nrOfTTPositions = 18;typedef struct { int dccId; bool currentPosition; TTPositionSwitch ttPosition;} TurnoutSwitchTable;TurnoutSwitchTable TTPositions[nrOfTTPositions] = { {201, false, TTPositionSwitch(13, false)}, {202, false, TTPositionSwitch(12, false)}, {203, false, TTPositionSwitch(11, false)}, {204, false, TTPositionSwitch(10, false)}, {205, false, TTPositionSwitch( 9, false)}, {206, false, TTPositionSwitch( 8, false)}, {207, false, TTPositionSwitch( 7, false)}, {208, false, TTPositionSwitch( 6, false)}, {207, false, TTPositionSwitch( 5, false)}, {201, false, TTPositionSwitch( 4, true)}, {202, false, TTPositionSwitch( 3, true)}, {203, false, TTPositionSwitch( 2, true)}, {204, false, TTPositionSwitch(14, true)}, {205, false, TTPositionSwitch(15, true)}, {206, false, TTPositionSwitch(16, true)}, {207, false, TTPositionSwitch(17, true)}, {208, false, TTPositionSwitch(18, true)}, {209, false, TTPositionSwitch(19, true)}, };//// Table with the definition for the micro switches controlling // the doors of the locoshed.//const int nrOfDoors = 2;TurnoutSwitchTable doorSwitches[nrOfDoors] = { {108, false, TTPositionSwitch(A0, false)}, {109, false, TTPositionSwitch(A1, false)}};
// // Activate the TT position switches and store the current state // for (int i=0; i<nrOfTTPositions; i++) { (TTPositions[i].ttPosition).attach(); TTPositions[i].currentPosition = (TTPositions[i].ttPosition).getState(); // // Checkt the state, and if active inform the TT controller // if ((TTPositions[i].ttPosition).getState()) { // // Send the initial position to the TT controller. // dccAccHandler(TTPositions[i].dccId, (TTPositions[i].ttPosition).getReverseDirection()); } } // // Activate the door switches and store the current state // for (int i=0; i<nrOfTTPositions; i++) { (doorSwitches[i].ttPosition).attach(); doorSwitches[i].currentPosition = (doorSwitches[i].ttPosition).getState(); // // Send the initial state to the TT controller. // dccAccHandler(doorSwitches[i].dccId, !(doorSwitches[i].ttPosition).getState()); }
for (int i = 0; i < nrOfTTPositions; i++) { // // Force an update of the switch // (TTPositions[i].ttPosition).heartBeat(); // // If this Position is changed, send the proper DCC accessory message // if (TTPositions[i].currentPosition != (TTPositions[i].ttPosition).getState()) { TTPositions[i].currentPosition = (TTPositions[i].ttPosition).getState(); if ((TTPositions[i].ttPosition).getState()) { // // The state of the rotational switch has changed, so send the new position // to the TT controller. // dccAccHandler(TTPositions[i].dccId, (TTPositions[i].ttPosition).getReverseDirection()); } } }
for (int i = 0; i < nrOfDoors; i++) { // // Force an update of the switch // (doorSwitches[i].ttPosition).heartBeat(); // // If this Position is changed, send the proper DCC accessory message // if (doorSwitches[i].currentPosition != (doorSwitches[i].ttPosition).getState()) { doorSwitches[i].currentPosition = (doorSwitches[i].ttPosition).getState(); // // The state of the door switch has changed, so send the new position // to the TT controller. // dccAccHandler(doorSwitches[i].dccId, !(doorSwitches[i].ttPosition).getState()); } }
void dccAccHandler(int dccId, bool aDirection){ // // Create the Accessory message // DCC_ACC_MSG buf; buf.address = dccId; if (!aDirection) { buf.direction = 0; } else { buf.direction = 1; } // // Send the received accessory command // canBus->sendMsg(DCC_ACC, sizeof(DCC_ACC_MSG), &buf);}
/* A Library to control turnouts (switches) Copyright (C) Meino de Graaf, all rights reserved This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2 of the License. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. */#include <Arduino.h>#include "KB_Servo.h"#define TO_DEBUG 0#define TURNOUT_STRAIGHT false#define TURNOUT_DIVERGING true//// Type definition for the call-back routine which will handle a state change//typedef void (*toStateHandler_t)(int toDccAddress, bool toDirection);//// The abstract base class for handling turnouts//class Turnout{protected: int toPin; // Arduino pin connected to the relay bool targetDirection; // Straight or Diverging bool currentDirection; int dccAddress; // DCC address used for this turnout toStateHandler_t toStateHandler = nullptr; void initTo(int newDccAddress, int newToPin);public: void setTargetDirection(bool newDirection); bool getCurrentDirection(); int getDccAddress(); void registerToStateHandler(toStateHandler_t toStateHandler); virtual void attach(bool initialDirection) = 0; virtual void heartBeat() = 0;};//// Class for turnouts controlled by a relay.//class RelayTurnout : public Turnout{public: RelayTurnout(int newDccAddress, int newToPin); void attach(bool initialDirection); void heartBeat(); };//// Class for turnouts controlled by a servo.//class ServoTurnout : public Turnout{private: int straightPosition; int divergingPosition; int currentPosition; int stepDelay; ArmServo* turnoutServo; bool servoRunning = false; public: ServoTurnout(int newDccAddress, int newToPin, int newStraightPosition, int newDivergingPosition, int newStepDelay); void attach(bool initialDirection); void heartBeat(); };
/* A Library to control turnouts (switches) Copyright (C) Meino de Graaf, all rights reserved This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2 of the License. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. */#include <KB_Turnout.h>void Turnout::initTo(int newDccAddress, int newRelayPin){ toPin = newRelayPin; dccAddress = newDccAddress; targetDirection = TURNOUT_STRAIGHT; currentDirection = TURNOUT_STRAIGHT;}bool Turnout::getCurrentDirection(){ return currentDirection;}int Turnout::getDccAddress(){ return dccAddress;}void Turnout::registerToStateHandler(toStateHandler_t aToStateHandler){ toStateHandler = aToStateHandler;}void Turnout::setTargetDirection(bool newDirection){ // // When the new direction differs from the current direction, update the turnout direction // and set the relayPin according to the new direction. // if (targetDirection != newDirection) { targetDirection = newDirection; // Set target direction #if (TO_DEBUG) Serial.print("[RelayTurnout::setTurnoutDirection] DCC address: "); Serial.print(dccAddress); Serial.print(", new direction: "); if (targetDirection == TURNOUT_STRAIGHT) { Serial.println(" -"); } else { Serial.println(" /"); } Serial.flush();#endif }}RelayTurnout::RelayTurnout(int newDccAddress, int newRelayPin){ initTo(newDccAddress, newRelayPin);}void RelayTurnout::attach(bool initialDirection) // Intialize the Relay{#if (TO_DEBUG) Serial.print("[RelayTurnout::attach] pin: "); Serial.print(toPin);#endif //DEBUG pinMode(toPin, OUTPUT); #if (TO_DEBUG) Serial.println(" attached"); Serial.flush();#endif //DEBUG // // Initialize the turnout in a fixed direction. // targetDirection = initialDirection; currentDirection = initialDirection; // // Force the initial direction // if (currentDirection == TURNOUT_STRAIGHT) { digitalWrite(toPin, HIGH); } else { digitalWrite(toPin, LOW); } // // give it some time to settle down // delay(100);#if (TO_DEBUG) Serial.print("[RelayTurnout::heartBeat] DCC address: "); Serial.print(dccAddress); if (currentDirection == TURNOUT_STRAIGHT) { Serial.println(" -"); } else { Serial.println(" /"); } Serial.flush();#endif // // Invoke the state handler // if (toStateHandler != nullptr) { toStateHandler(dccAddress, currentDirection); }}void RelayTurnout::heartBeat(){ if (currentDirection != targetDirection) { currentDirection = targetDirection; // Make target direction current direction if (currentDirection == TURNOUT_STRAIGHT) { digitalWrite(toPin, HIGH); } else { digitalWrite(toPin, LOW); } #if (TO_DEBUG) Serial.print("[RelayTurnout::heartBeat] DCC address: "); Serial.print(dccAddress); if (currentDirection == TURNOUT_STRAIGHT) { Serial.println(" -"); } else { Serial.println(" /"); } Serial.flush();#endif // // Invoke the state handler // if (toStateHandler != nullptr) { toStateHandler(dccAddress, currentDirection); } }}ServoTurnout::ServoTurnout(int newDccAddress, int newServoPin, int newStraightPosition, int newDivergingPosition, int newStepDelay){ initTo(newDccAddress, newServoPin); straightPosition = newStraightPosition; divergingPosition = newDivergingPosition; stepDelay = newStepDelay; /* * An ArmServo is used, because it has the proper functionality */ turnoutServo = new ArmServo(newServoPin, newStepDelay, newStraightPosition, newDivergingPosition, true);}void ServoTurnout::attach(bool initialDirection) // Intialize the Servo{ // // Initialize the turnout in a fixed direction. // targetDirection = initialDirection; currentDirection = initialDirection; // // Initialize the position of the servo arm // if (initialDirection == TURNOUT_STRAIGHT) { turnoutServo->initArmHigh(); } else { turnoutServo->initArmLow(); } // // Invoke the state handler // if (toStateHandler != nullptr) { toStateHandler(dccAddress, currentDirection); }}void ServoTurnout::heartBeat(){ if (currentDirection != targetDirection) { // // Start the movement // currentDirection = targetDirection; // Make target direction current direction if (currentDirection == TURNOUT_STRAIGHT) { turnoutServo->armHigh(); } else { turnoutServo->armLow(); } // // We start a movement, when finished the state need to be reported // servoRunning = true; } // // Keep the servo running // turnoutServo->heartBeat(); // // When the servo has reached it's final position, we have to invoke // the state handler // if (servoRunning) { if (((currentDirection == TURNOUT_STRAIGHT) && turnoutServo->isHigh()) || ((currentDirection == TURNOUT_DIVERGING) && turnoutServo->isLow())) { // // Invoke the state handler // if (toStateHandler != nullptr) { toStateHandler(dccAddress, currentDirection); } servoRunning = false; } }}
#pragma once/* A library to control servos used for semaphores and doors Copyright (C) Meino de Graaf, all rights reserved This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2 of the License. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. */#define PWM_SERVO 0#if (PWM_SERVO)#include <PWMServo.h>#else#include <Servo.h>#endif // PWM_SERVO//// Some definitions for convenience//#define PWM_PIN(r) (r)/* A wrapper class for controlling RC servo's */enum servoActivity { IDLE = 1, HIGHPOS = 2, HIGHBOUNCE1 = 3, HIGHBOUNCE2 = 4, HIGHBOUNCE3 = 5, LOWPOS = 6, LOWBOUNCE1 = 7, LOWBOUNCE2 = 8, LOWBOUNCE3 = 9};class ArmServo {private:#define REFRESH_TIME 60000#define DETACH_WAIT_TIME 200#if (PWM_SERVO) PWMServo *myServo;#else Servo *myServo;#endif //PWM_SERVO bool bounce; unsigned long timeOfLastStep; unsigned long detachWaitTime; bool detachable; bool needRefresh; unsigned long lastTimeRefreshed; unsigned int target; unsigned int current; unsigned int highPosition; // Arm position high unsigned int lowPosition; // Arm position low unsigned int stepWaitTime; // The wait time between steps unsigned int servoWaitTime; // Initial wait time between 2 steps servoActivity state = IDLE; int servoPin; bool active; // // Activate the servo by attaching it's servo pin. // void attachServo(); // // Deactivate the servo by detaching it's servo pin // void detachServo();public: // // Creator for a new instance of an ArmServo // // Parameters: // int newServoPin // Arduino pin used for controlling the servo // int newWaitTime // Time in mS between to consequtive servo steps // int newHighPosition // Servo position for the semaphore arm in it's high position // int newLowPosition // Servo position for the semaphore arm in it's low position // ArmServo(int newServoPin, int newWaitTime, int newHighPosition, int newLowPosition); // // Creator for a new instance of an ArmServo // // Parameters: // int newServoPin // Arduino pin used for controlling the servo // int newWaitTime // Time in mS between to consequtive servo steps // int newHighPosition // Servo position for the semaphore arm in it's high position // int newLowPosition // Servo position for the semaphore arm in it's low position // bool newNeedRefresh // When true, servo needs to be refreshed (repositioned) at regular // intervals // ArmServo(int newServoPin, int newWaitTime, int newHighPosition, int newLowPosition, bool newNeedRefresh); // // Creator for a new instance of an ArmServo // // Parameters: // int newServoPin // Arduino pin used for controlling the servo // int newWaitTime // Time in mS between to consequtive servo steps // int newHighPosition // Servo position for the semaphore arm in it's high position // int newLowPosition // Servo position for the semaphore arm in it's low position // bool newNeedRefresh // When true, servo needs to be refreshed (repositioned) at regular // intervals // bool aDetachable // When true, the servo is detached after a second when the target // position is reached. // //ArmServo(int newServoPin, int newWaitTime, int newHighPosition, int newLowPosition, bool newNeedRefresh, bool aDetachable); // // Set the target of the servo at it's defined low position. // void armLow(); // // Set the target and current possition of the servo at it's defined // low position. // void initArmLow(); // // If the servo is on it's defined low position, return true, else // return false. // bool isLow(); // // Set the target of the servo at it's defined high position. // void armHigh(); // // Set the target and current possition of the servo at it's defined // high position. // void initArmHigh(); // // If the servo is on it's defined high position, return true, else // return false. // bool isHigh(); // // Check the status of the servo. If it reached it's tarfet position // do nothing, else when the wait time has expired move it one step // further in the direction of the target. // bool heartBeat();};
/* A Library for controling servos used by semaphores and doors Copyright (C) Meino de Graaf, all rights reserved This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, version 2 of the License. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. */#include <stdlib.h>#include <arduino.h>#include <KB_Servo.h>//// A wrapper class to control RC servo's////// Activate the servo by attaching it's servo pin.//void ArmServo::attachServo() // Intialize the servo{ this->myServo->attach(this->servoPin); this->active = true; this->myServo->write(this->current); // Restore last known position}//// Deactivate the servo by detaching it's servo pin//void ArmServo::detachServo() { if ((this-detachable) && (this->active)) // skip when the servo is not // attached and not detachable { if (detachWaitTime == 0) // First time? intialize wait time { detachWaitTime = millis(); } else if ((millis()-detachWaitTime) >= DETACH_WAIT_TIME) // Waited long enough? { this->myServo->detach(); // Detach the servo this->active = false; } }}//// Creator for a new instance of an ArmServo//// Parameters:// int newServoPin // Arduino pin used for controlling the servo// int newWaitTime// Time in mS between to consequtive servo steps// int newHighPosition// Servo position for the semaphore arm in it's high position// int newLowPosition// Servo position for the semaphore arm in it's low position//ArmServo::ArmServo(int newServoPin, int newWaitTime, int newHighPosition, int newLowPosition) {#if (PWM_SERVO) this->myServo = new PWMServo();#else this->myServo = new Servo();#endif this->servoPin = newServoPin; this->servoWaitTime = newWaitTime; this->highPosition = newHighPosition; this->lowPosition = newLowPosition; this->active = false; this->state = LOWPOS; this->bounce = false; // disable bounce // // Set the arm at a default (LOW) position // this->target = newLowPosition; this->current = (2*newLowPosition+newHighPosition)/3; // Let the servo make a small movement // // This servo doesn't need to be repositioned // this->needRefresh = false; // // This servo doest need automatic detach // this->detachable = false;}//// Creator for a new instance of an ArmServo//// Parameters:// int newServoPin // Arduino pin used for controlling the servo// int newWaitTime// Time in mS between to consequtive servo steps// int newHighPosition// Servo position for the semaphore arm in it's high position// int newLowPosition// Servo position for the semaphore arm in it's low position// bool newNeedRefresh// When true, servo needs to be refreshed (repositioned) at regular// intervals//ArmServo::ArmServo(int newServoPin, int newWaitTime, int newHighPosition, int newLowPosition, bool newNeedRefresh) {#if (PWM_SERVO) this->myServo = new PWMServo();#else this->myServo = new Servo();#endif this->servoPin = newServoPin; this->servoWaitTime = newWaitTime; this->highPosition = newHighPosition; this->lowPosition = newLowPosition; this->active = false; this->state = LOWPOS; this->bounce = false; // disable bounce // // Set the arm at a default (LOW) position // this->target = newLowPosition; this->current = (2*newLowPosition+newHighPosition)/3; // Let the servo make a small movement // // This servo does need to be repositioned // this->needRefresh = newNeedRefresh; // // This setvo needs also an automatic detach // this->detachable = true;}//// Set the target of the servo at it's defined low position.//void ArmServo::armLow() { if (this->current != this->lowPosition) { this->target = this->lowPosition; //this->stepWaitTime = (this->servoWaitTime * 2) / 3; // The downwards movement is faster, gravity this->stepWaitTime = this->servoWaitTime; this->state = LOWPOS; }}//// Set the target and current possition of the servo at it's defined // low position.//void ArmServo::initArmLow() { this->current = this->lowPosition; this->target = this->lowPosition; this->stepWaitTime = this->servoWaitTime; this->lastTimeRefreshed = millis()-REFRESH_TIME-1; // force an immidiate refresh; this->state = IDLE;}//// If the servo is on it's defined low position, return true, else // return false.//bool ArmServo::isLow() { return (current == lowPosition);}//// Set the target of the servo at it's defined high position.//void ArmServo::armHigh() { if (this->current != this->highPosition) { this->target = this->highPosition; this->stepWaitTime = this->servoWaitTime; this->state = HIGHPOS; }}//// Set the target and current possition of the servo at it's defined // high position.//void ArmServo::initArmHigh() { this->current = this->highPosition; this->target = this->highPosition; this->stepWaitTime = this->servoWaitTime; this->lastTimeRefreshed = millis()-REFRESH_TIME-1; // force an immidiate refresh this->state = IDLE;}//// If the servo is on it's defined high position, return true, else // return false.//bool ArmServo::isHigh() { if (current == highPosition) return true; else return false;}//// Check the status of the servo. If it reached it's tarfet position// do nothing, else when the wait time has expired move it one step// further in the direction of the target.//bool ArmServo::heartBeat() { // // When the arm's position isn't at the target, we have to move the arm // if (this->target != this->current) { // // Is the servo active? if not attach it to it's pin so it can start moving // if (!this->active) { this->attachServo(); } this->lastTimeRefreshed = millis(); if ((this->lastTimeRefreshed - this->timeOfLastStep) >= this->stepWaitTime) { // // The servo is moving to it's new location. // if (this->target > this->current) { this->current++; } else { this->current--; } this->myServo->write(this->current); this->timeOfLastStep = lastTimeRefreshed; // // A little thing, force a refresh after 100msec // this->lastTimeRefreshed = lastTimeRefreshed-REFRESH_TIME+250; } return true; // The arm is still moving } else // The arm is at it's target position { switch (this->state) // Arm has reached it's target position, the follow-up depends on it's state { case IDLE: { // // Detached servo's can creep to a different position, so // at regular intervals we reset the current position. // if ((this->needRefresh) && ((millis() - this->lastTimeRefreshed) >= REFRESH_TIME)) { // // The actual movement of the servo is done in the // regular non idle state. so we set everything so, that // it wil activate that in the non-idle part of the heartbeat. // In this situation we have to wait 200ms before we can // continue. // // Is the servo active? if not attach it to it's pin so it can start moving // if (!this->active) { this->attachServo(); } this->myServo->write(this->current); this->lastTimeRefreshed = millis(); this->timeOfLastStep = millis(); // // Set the proper state // if (this->target == this->highPosition) { this->state = HIGHPOS; } else { this->state = LOWPOS; } detachWaitTime = 0; // Reset wait for actual detach return true; // The arm is moving } else { // // The arm reached it's position, so we can now detach the servo (if it was attached) // if (this->active) { this->detachServo(); } return false; // The arm is not moving } } // IDLE case HIGHPOS: { if (!this->bounce) { this->state = IDLE; return false; } // // We reached the High position, we are now starting the bounce // sequence. Calculate the new position // if (this->highPosition > this->lowPosition) { this->target = this->highPosition - ((this->highPosition - this->lowPosition) / 4); } else { this->target = this->highPosition + ((this->lowPosition - this->highPosition) / 4); } this->stepWaitTime = this->servoWaitTime / 2; this->state = HIGHBOUNCE1; return true; } // HIGHPOS case HIGHBOUNCE1: { if (this->current != this->highPosition) { this->target = this->highPosition; } else { if (this->highPosition > this->lowPosition) { this->target = this->highPosition - ((this->highPosition - this->lowPosition) / 6); } else { this->target = this->highPosition + ((this->lowPosition - this->highPosition) / 6); } this->state = HIGHBOUNCE2; } return true; } // HIGHBOUNCE1 case HIGHBOUNCE2: { if (this->current != this->highPosition) { this->target = this->highPosition; } else { if (this->highPosition > this->lowPosition) { this->target = this->highPosition - ((this->highPosition - this->lowPosition) / 8); } else { this->target = this->highPosition + ((this->lowPosition - this->highPosition) / 8); } this->state = HIGHBOUNCE3; } return true; } // HIGHBOUNCE2 case HIGHBOUNCE3: { if (this->current != this->highPosition) { this->target = this->highPosition; detachWaitTime = 0; // Reset wait for actual detach return true; } else { // // The arm reached it's position, so we can now detach the servo (if it was attached) // if (this->active) { this->detachServo(); } this->state = IDLE; } return false; case LOWPOS: if (!this->bounce) { this->state = IDLE; return false; } // // We reached the High position, we are now starting the bounce // sequence. Calculate the new position // if (this->lowPosition > this->highPosition) { this->target = this->lowPosition - ((this->lowPosition - this->highPosition) / 4); } else { this->target = this->lowPosition + ((this->highPosition - this->lowPosition) / 4); } stepWaitTime = servoWaitTime / 2; this->state = LOWBOUNCE1; return true; } // HIGHBOUNCE3 case LOWBOUNCE1: { if (this->current != this->lowPosition) { this->target = this->lowPosition; } else { if (this->lowPosition > this->highPosition) { this->target = this->lowPosition - ((this->lowPosition - this->highPosition) / 6); } else { this->target = this->lowPosition + ((this->highPosition - this->lowPosition) / 6); } this->state = LOWBOUNCE2; } return true; } // LOWBOUNCE1 case LOWBOUNCE2: { if (this->current != this->lowPosition) { this->target = this->lowPosition; } else { if (this->lowPosition > this->highPosition) { this->target = this->lowPosition - ((this->lowPosition - this->highPosition) / 8); } else { this->target = lowPosition + ((this->highPosition - this->lowPosition) / 8); } this->state = LOWBOUNCE3; } return true; } // LOWBOUNCE2 case LOWBOUNCE3: { if (this->current != this->lowPosition) { this->target = this->lowPosition; detachWaitTime = 0; // Reset wait for actual detach return true; } else { // // The arm reached it's position, so we can now detach the servo (if it was attached) // if (this->active) { this->detachServo(); } state = IDLE; } return false; } // LOWBOUNCE3 } }}