/* ServoM-Prog-s01-r01.ino Martin Jakobsen 29. marts 2017 Styring af servomotor hvor bevaegelsens yderpunkter og hastighed kan indstilles. This code is in the public domain for non-commercial use. */ #include #include const int aktiveringPinA = 2; // Pin tilsluttet kontakt der aktiverer bevaegelse af motoren const int aktiveringPinB = 3; // Pin tilsluttet kontakt der aktiverer bevaegelse af motoren const int ledPinZ = 4; // Pin for LED af indikering af motor position const int ledPinX = 5; // Pin for LED af indikering af motor position const int modePinMT = 6; // Pin tilsluttet dip-sw til mode valg. M = mono T = toggle const int modePinAB = 7; // Pin tilsluttet dip-sw til valg af A eller AB mode const int justPinZ = 8; // Pin tilsluttet dip-sw til valg af justering af Z position const int justPinX = 9; // Pin tilsluttet dip-sw til valg af justering af X position const int ledPinF1 = 10; // Pin ned blink tilsluttet LED F1 (gul) const int ledPinF2 = 11; // Pin ned blink tilsluttet LED F2 (groen) const int servoPin = 12; // Pin hvor servomotoren er tilsluttet const int ledPin13 = 13; // Pin for indbygget LED L. Lyser hvis Z X position er fejlindstillet int potpinX = A0; // Analog Pin tilsluttet potentiometer for lagring af X position int potpinZ = A1; // Analog Pin tilsluttet potentiometer for lagring af Z position int potpinH = A2; // Analog Pin tilsluttet potentiometer for lagring af H hastighed int aktiveretT = 0; // Skal vaere 0 ved togle funktion foer nyt skifte kan foregaa int aktiveringStateA = 1; // Variabel for laesning af kontakt status aktiverings kontakt A int aktiveringStateB = 1; // Variabel for laesning af kontakt status aktiverings kontakt B int modeStateMT = 0; // Variabel for laesning af DIP-sw for mode Mono eller Toggle drift int modeStateAB = 0; // Variabel for laesning af DIP-sw for mode A eller AB drift int justStateZ = 0; // Variabel for laesning af DIP-sw for justering af Z position int justStateX = 0; // Variabel for laesning af DIP-sw for justering af X position int motorStilling = 0; // Variabel for servoens position. 0 er Z og 1 er X int pos = 0; // Variabel for gemme servo positionen under bevaegelse int valZ; // variabel for laesning fra den analoge potmeter Z pin int valX; // variabel for laesning fra den analoge potmeter X pin int valH; // variabel for laesning fra den analoge potmeter H pin int valueEE; // variabel for laesning af indhold i eeprom int counterDb = 0; // Debounse int readingDb; // Debounse long timeDb = 0; // Debounse int debounce_count = 50; // Antal milisekunder foer indgangen er erklaeret debounsed int ledState = LOW; // Variabel for blink LED tilstand unsigned long previousMillis = 0; // Gemmer tispunkt for blink LED var opdateret const long interval = 900; // Tid mellem blink - (millisekunder) Servo myservo; // Skaber servo objekt for kontrol af en servomotor void setup() { myservo.attach(12); // Aktiverer servoen paa pin 12 til servo objektet valueEE = EEPROM.read(1); // Laeser motor stilling i eeprom pinMode(aktiveringPinA, INPUT_PULLUP); // Initialiserer pin som input fra kontakt A pinMode(aktiveringPinB, INPUT_PULLUP); // Initialiserer pin som input fra kontakt B pinMode(justPinX, INPUT_PULLUP); // Initialiserer pin som input fra DIP-sw justering af X position pinMode(justPinZ, INPUT_PULLUP); // Initialiserer pin som input fra DIP-sw justering af Z position pinMode(modePinAB, INPUT_PULLUP); // Initial pin som input fra DIP-sw valg af A eller AB mode pinMode(modePinMT, INPUT_PULLUP); // Initial pin som input fra DIP-sw valg af M eller T mode pinMode(ledPinZ, OUTPUT); // Initialiserer pin tilsluttet LED for visning af motor position Z pinMode(ledPinX, OUTPUT); // Initialiserer pin tilsluttet LED for visning af motor position X pinMode(ledPinF1, OUTPUT); // Saetter digital blink pin til output (Gul) LED pinMode(ledPinF2, OUTPUT); // Saetter digital blink pin til output (Groen) LED pinMode(ledPin13, OUTPUT); // LED paa pin 13 // laesning af motorstilling ved start (potentiometer vaerdier) valZ = analogRead(potpinZ); // Laeser vaerdien af potentiometert (vaerdi mellem 0 og 1023) valZ = map(valZ, 0, 1023, 0, 180); // Skalerer det til brug for servoen (vaerdi nellem 0 og optil 180) valX = analogRead(potpinX); // Laeser vaerdien af potentiometert (vaerdi mellem 0 og 1023) valX = map(valX, 0, 1023, 0, 180); // Skalerer det til brug for servoen (vaerdi nellem 0 og optil 180) valH = analogRead(potpinH); // Laeser vaerdien af potentiometeret for hastighed valH = map(valH, 0, 1023, 0, 200); // Skalerer det til brug for servoen // Soerger for at motoren bliver i positionen fra foer sluk/reset af systemet if (valueEE == 0) // Hvis indholdet af eeprom er 0 er motoren i Z position { myservo.write(valZ); // Soerger for at start positionen er korrekt motorStilling = 0; // Motorstilling er Z } else if (valueEE == 1) // Hvis indholdet af eeprom er 1 er motoren i X position { myservo.write(valX); // Soerger for at start positionen er korrekt motorStilling = 1; // Motorstilling er X } Serial.begin(9600); // Laver seriel data forbindelse til PC Serial Monitor Serial.println("sw ServoM-Prog-s01-r01.ino Marts 2017 Programmet er startet"); // Skriver tekst Serial.print ("Potmeter vaerdi Z er "); Serial.println(valZ); Serial.print ("Potmeter vaerdi X er "); Serial.println(valX); Serial.print ("Potmeter vaerdi H er "); Serial.println(valH); if (valueEE == 0) { Serial.println ("Motorstilling er Z. EEprom indhold er 0. "); } if (valueEE == 1) { Serial.println ("Motorstilling er X. EEprom indhold er 1. "); } Serial.print ("\n"); // ekstra linjeskift } void loop() { unsigned long currentMillis = millis(); // blink start if (currentMillis - previousMillis >= interval) { previousMillis = currentMillis; // gemmer sidste gang LED blinkede if (ledState == LOW) // skifter LED taendt/slukket vice-versa { ledState = HIGH; } else { ledState = LOW; } // F1 eller F2 LED blinker if (motorStilling == 0) { digitalWrite(ledPinF1, ledState); digitalWrite(ledPinF2, LOW); } else if (motorStilling == 1) { digitalWrite(ledPinF2, ledState); digitalWrite(ledPinF1, LOW); } } // LED F1 eller F2 blinker (evt. gul groen til nodeljernbane forsignal) valH = analogRead(potpinH); // Laeser vaerdien af potentiometeret for hastighed valH = map(valH, 0, 1023, 0, 200); // Skalerer det til brug for servoen justStateZ = digitalRead(justPinZ); // Laeser status af DIP-sw for justering af Z position justStateX = digitalRead(justPinX); // Laeser status af DIP-sw for justering af X position if (justStateZ == LOW) // Indstilling af yderposition Z { valZ = analogRead(potpinZ); // Laeser potentiometers vaerdi (vaerdier mellem 0 og 1023) valZ = map(valZ, 0, 1023, 0, 180); // Skalerer til servo 0-180 grader bevaegelse myservo.write(valZ); // Saetter positionen i foelge den skalerede vaerdi } else if (justStateX == LOW) // Indstilling af yderposition X { valX = analogRead(potpinX); // Laeser potentiometers vaerdi (vaerdier mellem 0 og 1023) valX = map(valX, 0, 1023, 0, 180); // Skalerer til servo 0-180 grader bevaegelse myservo.write(valX); // Saetter positionen i foelge den skalerede vaerdi } else { // LED viser motorstilling if (motorStilling == 0) { digitalWrite(ledPinZ, HIGH); digitalWrite(ledPinX, LOW); } else if (motorStilling == 1) { digitalWrite(ledPinZ, LOW); digitalWrite(ledPinX, HIGH); } valZ = analogRead(potpinZ); // Laeser potentiometers vaerdi (vaerdier mellem 0 og 1023) valZ = map(valZ, 0, 1023, 0, 180); // Skalerer til servo 0-180 grader bevaegelse valX = analogRead(potpinX); // Laeser potentiometers vaerdi (vaerdier mellem 0 og 1023) valX = map(valX, 0, 1023, 0, 180); // Skalerer til servo 0-180 grader bevaegelse if (valX > valZ) { digitalWrite(ledPin13, LOW); } // Slukket LED L paa Arduino ben 13 if (valX < valZ) { digitalWrite(ledPin13, ledState); } // LED 13 blinker hvis potm inst overlapper modeStateMT = digitalRead(modePinMT); // Laeser DIP-sw kontaktens tilstand modeStateAB = digitalRead(modePinAB); // Laeser DIP-sw kontaktens tilstand aktiveringStateB = digitalRead(aktiveringPinB); // Laeser aktiverings kontaktens tilstand // aktiveringStateA = digitalRead(aktiveringPinA); // Laeser akt. kontaktens tilstand uden debounce if(millis() != timeDb) // Debounce start. Hindrer utilsigtet toggle funktion ved prel paa kontakt A { readingDb = digitalRead(aktiveringPinA); if(readingDb == aktiveringStateA && counterDb > 0) { counterDb--; } if(readingDb != aktiveringStateA) { counterDb++; } if(counterDb >= debounce_count) // Hvis ok laenge nok, skifter vi { counterDb = 0; aktiveringStateA = readingDb; // Laeser aktiverings kontaktens tilstand m. debounce } timeDb = millis(); } // Debounce slut } if (motorStilling == 0 && aktiveretT == 0 && aktiveringStateA == LOW) { // Hvis tilstand er Z skal der kores mod X naar A aktiveres aktiveretT = 1; // Togle funktion er i gang for(pos = valZ; pos < valX; pos=pos+1) // Gaar fra 0 grader til 180 grader i trin af 1 grad { myservo.write(pos); // Servoen skal gaa til positionen i variabel pos delay(valH); // Forsinkelse for hastighed indstillet i potmeter H } motorStilling = 1; // Ny tilstand saettes til X Serial.print("Ny motorstilling er X. Der skrives 1 i EEprom.\n"); // skriver til PC EEPROM.write(1, 1); // skriver motorens stilling i EEprom } else if (motorStilling == 1 && aktiveretT == 0) // Hvis tilstand er X skal der kores mod Z { if ((modeStateAB == LOW && aktiveringStateB == LOW) ||(modeStateAB == HIGH && modeStateMT == LOW && aktiveringStateA == LOW) ||(modeStateAB == HIGH && modeStateMT == HIGH && aktiveringStateA == HIGH)) { aktiveretT = 1; // Togle funktion er i gang for(pos = valX; pos>=valZ; pos=pos-1) // Gaaar fra 180 grader til 0 grader i trin af 1 grad { myservo.write(pos); // Servoen skal gaa til positionen i variabel pos delay(valH); // Forsinkelse for hastighed indstillet i potmeter H } motorStilling = 0; // Ny tilstand saettes til Z Serial.print("Ny motorstilling er Z. Der skrives 0 i EEprom.\n"); // skrives til PC EEPROM.write(1, 0); // skriver motorens stilling i EEprom } } if (aktiveringStateA == HIGH) // aktivering A er afbrudt { aktiveretT = 0; // Der er klar til ny togle funktion } } // slut paa loeken