/**************************************************************************************************************************************************
Servo-Ansteuerung mit Nachwippen für Formsignale
V_1.2
domapi
18.12.2019
***************************************************************************************************************************************************
Features:
- Mehrere Servos (aktuell bis zu 4) lassen sich unabhängig voneinander über jeweils 2 Taster pro Servo oder über DCC-Kommandos steuern.
- Der Sketch verwendet keine Delay-Funktionen, alles wird über millis() ge-timed.
- Mehrere Servos können parallel laufen!
- Einstellbar sind pro Servo (aktuell muss hier der Code angepasst werden):
- Endpositionen
- Geschwindigkeit (gilt aktuell für beide Richtungen)
- Anzahl Nachwipp-Bewegungen
- Geschwindigkeit und Stärke des Nachwippens
ToDos:
- evtl. Unterroutinen/Fkt. definieren
- Check, ob Position A > B ist, einbauen und dann ggf. die Positionen tauschen. Aktuell muss A < B sein, sonst gibt es Fehler im Ablauf
- DCC: CVs lesen, schreiben
- Alle Einstellungen als CVs vorsehen
Hardware:
---------
- Servos direkt an 0V, +5V und an den jeweiligen nano-Pin für das PWM-Signal anschließen
- Bis zu 2 Servos können direkt über USB versorgt werden, andernfalls separates 5V-Netzteil verwenden
(oder Brückengleichrichter und 7805 Spannungsregler)
- Je ein Widerstand 4,7 k zwischen +5 V und Servo-PWM-Leitung hilft manchmal gegen Einschaltzucken
und/oder "Bewegungszucken"
- Taster direkt zwischen GND und den jeweiligen Arduino-Pin anschließen, interne Pullup-Widerstände werden genutzt. Taster schalten auf LOW/GND.
- Auswertung DCC-Signal über 6N137 Optokoppler, Schutzdiode (1N4148) und Vorwiderstand (1k).
Pin 6 mit 10kOhm an 5V reicht aus. Pin 7 muss nicht an 5V angeschlossen sein, er kann unbelegt bleiben.
- ACK-Signal ggf. später ´mal über Optokoppler und Transistor anschließen
Arduino Nano:
// +-----+
// +------------| USB |------------+
// | +-----+ |
// LED | [ ]D13/SCK MISO/D12[ ] | Servo [2] PWM-Pin des Servos
// | [ ]3.3V MOSI/D11[ ]~| Servo [1]
// | [ ]V.ref ___ SS/D10[ ]~| Taste_B[4] Tasten alle an GND/LOW
//Servo[3] | [ ]A0 / N D9[ ]~| Taste_A[4]
//Servo[4] | [ ]A1 / A D8[ ] | Taste_B[3]
// | [ ]A2 N / D7[ ] | Taste_A[3]
// | [ ]A3 _0_/ D6[ ]~| Taste_B[2]
// | [ ]A4/SDA D5[ ]~| Taste_A[2]
// | [ ]A5/SCL D4[ ] | Taste_B[1]
// | [ ]A6 INT1/D3[ ]~| Taste_A[1]
// | [ ]A7 INT0/D2[ ] | DCC-Eingang über Optokoppler Pin 6
// | [ ]5V GND[ ] |
// | [ ]RST RST[ ] |
// | [ ]GND 5V MOSI GND TX1[ ] |
// | [ ]Vin [ ] [ ] [ ] RX1[ ] |
// | [ ] [ ] [ ] |
// | MISO SCK RST |
// | NANO-V3 |
// +-------------------------------+
*/
//=====================================================================================================
// Libraries einbinden, machen das Leben leichter
#include <Servo.h>
#include <EEPROM.h>
#include <NmraDcc.h>
//===========================================================================================================================
//#define POS_DEBUG 1 // Auskommentieren, wenn keine Ausgabe der Positionen auf den seriellen Monitor erfolgen soll
// Die Positionen des Servos lassen sich übrigens prima im seriellen Plotter als Kurve darstellen!
#define DCC_Stuff 1 // in finaler Version auskommentieren
//----------------------------------------------------------------------------------------------------------------------------
// Definitionen aus der NMRADCC-Bibliothek
//----------------------------------------------------------------------------------------------------------------------------
NmraDcc Dcc;
DCC_MSG Packet;
struct CVPair
{
int CV;
byte Value;
};
CVPair FactoryDefaultCVs [] =
{
{CV_ACCESSORY_DECODER_ADDRESS_LSB, 57},
{CV_ACCESSORY_DECODER_ADDRESS_MSB, 0},
};
byte FactoryDefaultCVIndex = 0;
void notifyCVResetFactoryDefault()
{
// Make FactoryDefaultCVIndex non-zero and equal to num CV's to be reset
// to flag to the loop() function that a reset to Factory Defaults needs to be done
FactoryDefaultCVIndex = sizeof(FactoryDefaultCVs) / sizeof(CVPair);
};
const int DccAckPin = A3; // Hier kann später das Acknowledgement-Signal erzeugt werden (über einene Optokoppler CNY17 und einen Transistor)
// This function is called by the NmraDcc library when a DCC ACK needs to be sent
// Calling this function should cause an increased 60ma current drain on the power supply for 6ms to ACK a CV Read
void notifyCVAck(void)
{
#ifdef DCC_Stuff
Serial.println("notifyCVAck") ;
#endif
digitalWrite( DccAckPin, HIGH );
delay( 6 );
digitalWrite( DccAckPin, LOW );
}
//----------------------------------------------------------------------------------------------------------------------------
// Variablen und Konstanten für die Bewegung der Servos
const int servos = 4; // Anzahl der angeschlossenen Servos, aktuell 4 Stück
Servo Servo[servos]; // Definition der Servos als Array
int servo_pins[servos] = {11, 12, A0, A1}; // an diese Arduino-Pins werden die Servo-PWM-Leitungen angeschlossen
// Servopositionen und -Geschwindigkeit; kommen später mal ins EEPROM als CVs
byte pos_A[servos] = {89, 106, 90, 90}; // rechte Endposition der Servos (Draufsicht)
byte pos_B[servos] = {135, 130, 120, 120}; // linke Endposition der Servos
byte pos[servos]; // Aktuelle Servoposition
byte Geschwindigkeit[servos] = {20, 20, 5, 5}; // Anzahl ms zwischen zwei aufeinanderfolgenden Positionen bei Bewegung
//-----------------------------------------------------------------------------------------------------------------------------
//Parameter für das Wippen
byte Wippen[servos][10] = // hier zehn Werte für 10 mal Nachwippen maximal, Angabe pro Servo in Grad ab der Endposition
{
{14, 11, 9, 5, 4, 3, 2, 2, 1, 1}, // bestimmt die Stärke das Nachwippens
{8, 7, 6, 5, 4, 3, 3, 3, 2, 1}, // normalerweise nimmt die Weite des Ausschlags mit der Zeit ab
{14, 11, 9, 5, 4, 3, 2, 2, 1, 1},
{14, 11, 9, 5, 4, 3, 2, 2, 1, 1},
} ;
byte anzahl_wippen[servos] = {6, 4, 5, 5}; // Anzahl pro Servo, wie oft nachgewippt werden soll
// Nimmt im Sketch dann die letzten x Werte aus dem Array "Wippen" oben
byte v_wippen[servos] = {30, 20, 25, 25}; // Geschwindigkeit des Nachwippens, sollte etwas langsamer als die Servogeschwindigkeit sein
byte state[servos]; // State machine für Servobewegung
byte wipp_state[servos]; // State machine für Nachwipp-Bewegung
unsigned long t_aktuell = 0; // Aktueller Zeitstempel, wird für die millis()-Auswertung benötigt
unsigned long t_alt[servos]; // Vorheriger Zeitstempel, wird für die millis()-Auswertung benötigt
int i = 0; // Zähler für Wipp-Bewegungen
int j[servos];
//----------------------------------------------------------------------------------------------------------------------------
// Variablen für das Blinken der Status-LED
const byte LedPin = LED_BUILTIN; // ist die eingebaute LED sn Pin 13 im Nano ;-)
unsigned long t_LED = 0;
int LED_on = 50;
int LED_off = 950; // Off-Zeit bei Stillstand --> langsames Blinken
int LED_off_schnell = 150; // Off-Zeit bei Servobewegung --> schnelles Blinken
byte LED_Status = LOW;
boolean servo_in_motion = false; // "true", sobald ein Servo in Bewegung ist
//----------------------------------------------------------------------------------------------------------------------------
//Variablen zur Tastenabfrage
//zunächst die benötigten Nano-Pins
const byte Taste_A[servos] = {3, 5, 7, 9};
const byte Taste_B[servos] = {4, 6, 8, 10};
//dann ein paar Arrays, werden im Setup() unten gefüllt
byte Status_Taste_A[servos]; // Status der Taste: LOW = gedrückt, HIGH = offen
byte Status_Taste_B[servos];
byte letzter_Status_Taste_A[servos]; // letzter Status
byte letzter_Status_Taste_B[servos];
unsigned long t_A[servos]; // letzter Zeitpunkt des Tastendrucks für´s Entprellen
unsigned long t_B[servos];
//Variablen nur einmal notwendig
const int Entprellen_ms = 50;
int Aktueller_Tastenstatus;
//----------------------------------------------------------------------------------------------------------------------------
// Routine aus dem Accessory-Decoder Beispiel der NRMA-Bibliothek
//----------------------------------------------------------------------------------------------------------------------------
// This function is called whenever a normal DCC Turnout Packet is received and we're in Output Addressing Mode
void notifyDccAccTurnoutOutput( uint16_t Addr, uint8_t Direction, uint8_t OutputPower )
{
// die Zentrale sendet für Magnetartikel (accessories) immer ein Paket bestehend aus Adresse, Richtung und ein/aus.
// Zunächst wird eine Richtung eingeschaltet, dazu wird das DCC-Telegramm ggf. mehrfach wiederholt
// Bei der ECOS wird nach einer einstellbaren Zeit pro "Magnetartikel" ein Ausschaltbefehl hinterhergeschickt
// wir reagieren nur aufs Einschalten
// Nur auf DCC-Kommandos für das Einschalten reagieren
if (OutputPower == 1)
{
// Für alle Servos die Adresse prüfen und ggf. Bewegung starten. 1. Adresse ist derzeit die 57 fest verdrahtet.
for (i = 0; i < servos; i++)
{
if (Addr == 57 + i)
{
if (Direction == 0 && state[i] != 3 && pos[i] != pos_A[i])
{
state[i] = 3; // Bewegung nur dann starten, wenn nicht schon aktiv und nur wenn Endposition nicht erreicht
Servo[i].attach(servo_pins[i]);
#ifdef DCC_Stuff
Serial.print(millis());
Serial.print(" Signal-/Weichen-Adresse: ") ;
Serial.print(Addr, DEC) ; // ist die Signal-/Weichenadresse, z.B. "57", entspricht 1:1 dem Wert aus der ESU ECoS
Serial.print(" Richtung: ");
if (Direction == 0) Serial.print(" rot ");
if (Direction == 1) Serial.print(" grün");
if (OutputPower == 0) Serial.println(" Off");
if (OutputPower == 1) Serial.println(" On");
#endif
}
if (Direction == 1 && state[i] != 1 && pos[i] != pos_B[i])
{
state[i] = 1;
Servo[i].attach(servo_pins[i]);
#ifdef DCC_Stuff
Serial.print(millis());
Serial.print(" Signal-/Weichen-Adresse: ") ;
Serial.print(Addr, DEC) ; // ist die Signal-/Weichenadresse, z.B. "57" entspricht 1:1 dem Wert aus der ESU ECoS
Serial.print(" Richtung: ");
if (Direction == 0) Serial.print(" rot ");
if (Direction == 1) Serial.print(" grün");
if (OutputPower == 0) Serial.println(" Off");
if (OutputPower == 1) Serial.println(" On");
#endif
}
}
}
}
}
//=====================================================================================================
void setup()
{
pinMode(LedPin, OUTPUT);
#ifdef POS_DEBUG
Serial.begin(115200);
#endif
#ifdef DCC_Stuff
Serial.begin(115200);
#endif
// Configure the DCC CV Programing ACK pin for an output
pinMode( DccAckPin, OUTPUT );
// Setup which External Interrupt, the Pin it's associated with that we're using and enable the Pull-Up
Dcc.pin(0, 2, 1);
// Call the main DCC Init function to enable the DCC Receiver
Dcc.init( MAN_ID_DIY, 10, CV29_ACCESSORY_DECODER | CV29_OUTPUT_ADDRESS_MODE, 0 );
Serial.println("Domapi`s DCC Servodekoder mit Nachwippen V_1.2");
Serial.println("Ready for DCC commands ...");
for (i = 0; i < servos; i++)
{
Status_Taste_A[i] = HIGH;
Status_Taste_B[i] = HIGH;
letzter_Status_Taste_A[i] = HIGH;
letzter_Status_Taste_B[i] = HIGH;
t_A[i] = 0;
t_B[i] = 0;
t_alt[i] = 0;
pinMode(Taste_A[i], INPUT_PULLUP); // alle Tasten nutzen die internen Pullup-Widerstände, spart Bauteile und Lötaufwand ;-)
pinMode(Taste_B[i], INPUT_PULLUP);
pos[i] = EEPROM.read(i); // letzte Position einlesen
#ifdef POS_DEBUG
Serial.println (pos[i]);
#endif
Servo[i].attach(servo_pins[i]); // Servo-Pins festlegen
Servo[i].write(pos[i]);
delay(50);
Servo[i].detach();
}
}
//=====================================================================================================
void loop()
{
// You MUST call the NmraDcc.process() method frequently from the Arduino loop() function for correct library operation
Dcc.process();
// Das brauchen wir aktuell auch nicht:
// if ( FactoryDefaultCVIndex && Dcc.isSetCVReady())
// {
// FactoryDefaultCVIndex--; // Decrement first as initially it is the size of the array
// Dcc.setCV( FactoryDefaultCVs[FactoryDefaultCVIndex].CV, FactoryDefaultCVs[FactoryDefaultCVIndex].Value);
// }
// Aktuelle Zeit speichern
t_aktuell = millis();
//-----------------------------------------------------------------------------------------------------
LED_blinken(); //läßt die LED kurz aufblinken, wenn die Steuerung aktiv ist. Schnelles Blinken, wenn ein Servo bewegt wird.
//-----------------------------------------------------------------------------------------------------
// Tasten für alle Servos abfragen und entprellen
//-----------------------------------------------------------------------------------------------------
// Tastendruck in die entgegengesetzte Richtung beendet die erste Bewegung und fährt den Servo zurück
// Dauerkontakt führt die Bewegung aus, startet sie dann aber nicht erneut!
//-----------------------------------------------------------------------------------------------------
// Taste Position A,fährt Servo von B --> A
// Stellt Signal auf ROT
//-----------------------------------------------------------------------------------------------------
for (i = 0; i < servos; i++)
{
Aktueller_Tastenstatus = digitalRead(Taste_B[i]);
if ( Aktueller_Tastenstatus != letzter_Status_Taste_B[i]) // Taste gedrückt?
{
t_B[i] = t_aktuell; // Zeitpunkt merken
}
if (t_aktuell - t_B[i] > Entprellen_ms) // schon die Entprellzeit abgelaufen?
{
if (Aktueller_Tastenstatus != Status_Taste_B[i]) // immer noch gedrückt?
{
Status_Taste_B[i] = Aktueller_Tastenstatus;
if (Status_Taste_B[i] == LOW)
{
if (state[i] != 3 && pos[i] != pos_A[i])
{
state[i] = 3;
Servo[i].attach(servo_pins[i]);
}
}
}
}
letzter_Status_Taste_B[i] = Aktueller_Tastenstatus;
}
//-----------------------------------------------------------------------------------------------------
// Taste Position B, fährt Servo von A --> B
// stellt Signal auf GRÜN
//-----------------------------------------------------------------------------------------------------
for (i = 0; i < servos; i++)
{
Aktueller_Tastenstatus = digitalRead(Taste_A[i]);
if ( Aktueller_Tastenstatus != letzter_Status_Taste_A[i]) // Taste gedrückt?
{
t_A[i] = t_aktuell; // Zeitpunkt merken
}
if (t_aktuell - t_A[i] > Entprellen_ms) // schon die Entprellzeit abgelaufen?
{
if (Aktueller_Tastenstatus != Status_Taste_A[i]) // immer noch gedrückt?
{
Status_Taste_A[i] = Aktueller_Tastenstatus;
if (Status_Taste_A[i] == LOW)
{
if (state[i] != 1 && pos[i] != pos_B[i]) // Bewegung nur dann starten, wenn noch nicht aktiv!
{
state[i] = 1;
Servo[i].attach(servo_pins[i]);
}
}
}
}
letzter_Status_Taste_A[i] = Aktueller_Tastenstatus;
}
servo_in_motion = false;
// Prüfen, ob irgendein Servo gerade eine Bewegung ausführt. Dann soll die LED schnell blinken.
for (i = 0; i < servos; i++)
{
if (state[i] > 0)
{
servo_in_motion = true;
break; // Sobald der erste Servo gefunden wurde, kann man die Schleife verlassen!!! Spart echt Zeit ;-)
}
}
// Nun alle Servos parallel mit den nötigen Positionsinfos ansteuern
// Quasi Multitasking. Die Dekoder werden nacheinander immer ein Stückchen weiter positioniert.
// Jeder Servo bekommt seine Zeitscheibe ...
// Jeder Servo durchläuft 4 states: 0 = Nix tun, 1 = Bewegung A --> B, 2 = Nachwippen in Pos. B, 3 = Bewegung B --> A, 4 = Nachwippen Pos. A
// Beim Nachwippen gibt´s nochmal einen weitern state: wipp_state: 1 = Rückwärtsbewegung, 2 = Vorwärtsbewegung
for (i = 0; i < servos; i++)
{
switch (state[i])
{
//-----------------------------------------------------------------------------------------------------
case 1:
// Bewegung von A --> B aktiv
if (t_aktuell - t_alt[i] > Geschwindigkeit[i])
{
t_alt[i] = t_aktuell;
// wenn Zeit abgelaufen, wieder eine Position weiter bewegen
pos[i]++;
if (pos[i] <= pos_B[i])
{
Servo[i].write(pos[i]);
#ifdef POS_DEBUG
Serial.println (pos[i]);
#endif
}
else
{
pos[i] = pos_B[i];
if (anzahl_wippen[i] == 0)
{
state[i] = 0;
Servo[i].detach(); //Servoimpuls abschalten, wenn Bewegung beendet
EEPROM.write(i, pos[i]);
}
else
{
state[i] = 2;
wipp_state[i] = 1;
j[i] = 10 - anzahl_wippen[i];
}
}
}
break;
//-----------------------------------------------------------------------------------------------------
case 2:
// Nachwippen an Position B
// Die Geschwindigkeit beim Nachwippen jedesmal verringern in Abhängigkeit von i
if (t_aktuell - t_alt[i] > v_wippen[i] + 2 * j[i])
{
t_alt[i] = t_aktuell;
//------------------------------------------------------------------------------------------------
switch (wipp_state[i])
{
case 1:
// wenn Zeit abgelaufen, wieder eine Position zurück bewegen
if (pos[i] >= pos_B[i] - Wippen[i][j[i]])
{
pos[i]--;
Servo[i].write(pos[i]);
#ifdef POS_DEBUG
Serial.println (pos[i]);
#endif
}
else
{
wipp_state[i] = 2;
}
break;
//-----------------------------------------------------------------------------------------------------
case 2:
// wenn Zeit abgelaufen, wieder eine Position vor bewegen
if (pos[i] < pos_B[i])
{
pos[i]++;
Servo[i].write(pos[i]);
#ifdef POS_DEBUG
Serial.println (pos[i]);
#endif
}
else
{
wipp_state[i] = 1;
if (j[i] < 9)
{
j[i]++;
}
else
{
j[i] = 10 - anzahl_wippen[i];
state[i] = 0;
EEPROM.write(i, pos[i]);
Servo[i].detach(); //Servoimpuls abschalten, wenn Bewegung beendet
}
}
break;
}
}
break;
//-----------------------------------------------------------------------------------------------------
case 3:
// Bewegung von B --> A aktiv
if (t_aktuell - t_alt[i] > Geschwindigkeit[i])
{
t_alt[i] = t_aktuell;
// wenn Zeit abgelaufen, wieder eine Position weiter bewegen
pos[i]--;
if (pos[i] >= pos_A[i])
{
Servo[i].write(pos[i]);
#ifdef POS_DEBUG
Serial.println (pos[i]);
#endif
}
else
{
pos[i] = pos_A[i];
if (anzahl_wippen[i] == 0)
{
state[i] = 0;
Servo[i].detach(); //Servoimpuls abschalten, wenn Bewegung beendet
EEPROM.write(i, pos[i]);
}
else
{
state[i] = 4;
wipp_state[i] = 1;
j[i] = 10 - anzahl_wippen[i];
}
}
}
break;
//-----------------------------------------------------------------------------------------------------
case 4:
// Nachwippen an Position A
if (t_aktuell - t_alt[i] > v_wippen[i] + 2 * j[i])
{
t_alt[i] = t_aktuell;
switch (wipp_state[i])
{
//-----------------------------------------------------------------------------------------------------
case 1:
// wenn Zeit abgelaufen, wieder eine Position zurück bewegen
if (pos[i] <= pos_A[i] + Wippen[i][j[i]])
{
pos[i]++;
Servo[i].write(pos[i]);
#ifdef POS_DEBUG
Serial.println (pos[i]);
#endif
}
else
{
wipp_state[i] = 2;
}
break;
//-----------------------------------------------------------------------------------------------------
case 2:
// wenn Zeit abgelaufen, wieder eine Position weiter bewegen
if (pos[i] > pos_A[i])
{
pos[i]--;
Servo[i].write(pos[i]);
#ifdef POS_DEBUG
Serial.println (pos[i]);
#endif
}
else
{
wipp_state[i] = 1;
if (j[i] < 9)
{
j[i]++;
}
else
{
j[i] = 10 - anzahl_wippen[i];
state[i] = 0;
Servo[i].detach();
EEPROM.write(i, pos[i]);
}
}
break;
}
}
break;
}
}
}
//=====================================================================================================
// Unterprogramme
//=====================================================================================================
void LED_blinken ()
{
// LED blinken lassen
if (LED_Status == LOW)
{
if (servo_in_motion)
{
if (t_aktuell - t_LED > LED_off_schnell)
{
digitalWrite(LedPin, HIGH);
t_LED = t_aktuell;
LED_Status = HIGH;
}
}
else
{
if (t_aktuell - t_LED > LED_off)
{
digitalWrite(LedPin, HIGH);
t_LED = t_aktuell;
LED_Status = HIGH;
}
}
}
else
{
if (t_aktuell - t_LED > LED_on)
{
digitalWrite(LedPin, LOW);
t_LED = t_aktuell;
LED_Status = LOW;
}
}
}
//----------------------------------------------------------------------------------------------------------------------------
// Weitere NMRA-DCC-Routinen, die aktuell nicht gebraucht werden
//----------------------------------------------------------------------------------------------------------------------------
//----------------------------------------------------------------------------------------------------------------------------
// Uncomment to print all DCC Packets
//void notifyDccMsg( DCC_MSG * Msg)
//{
// Serial.print("notifyDccMsg: ") ;
// for (uint8_t i = 0; i < Msg->Size; i++)
// {
// Serial.print(Msg->Data[i], BIN);
// Serial.write(' ');
// }
// Serial.println();
//}
//
//----------------------------------------------------------------------------------------------------------------------------
//// This function is called whenever a normal DCC Turnout Packet is received and we're in Board Addressing Mode
//void notifyDccAccTurnoutBoard( uint16_t BoardAddr, uint8_t OutputPair, uint8_t Direction, uint8_t OutputPower )
//{
// Serial.print("notifyDccAccTurnoutBoard: ") ;
// Serial.print(BoardAddr, DEC) ;
// Serial.print(',');
// Serial.print(OutputPair, DEC) ;
// Serial.print(',');
// Serial.print(Direction, DEC) ;
// Serial.print(',');
// Serial.println(OutputPower, HEX) ;
//}
//
//----------------------------------------------------------------------------------------------------------------------------
//// This function is called whenever a DCC Signal Aspect Packet is received
//void notifyDccSigOutputState( uint16_t Addr, uint8_t State)
//{
// Serial.print("notifyDccSigOutputState: ") ;
// Serial.print(Addr, DEC) ;
// Serial.print(',');
// Serial.println(State, HEX) ;
//}