// A kapcsolo allasatol fuggoen
// ketto szervo helyzetet vezerli potenciometerrel (valtoztathato ellenallas),
// vagy program vezerli a szervokat es a robotkar vegen levo elektromagnest.
// 2012, Copyright Santa Krisztian, Csanyi Pal

#include <Servo.h> 

Servo szervo1;  // szervo1 objektum letrehozasa az egyik szervo vezerlesehez 
Servo szervo2;  // szervo2 objektum letrehozasa a masik szervo vezerlesehez

int potTu1 = 0;  // a 0-ik analog ture csatlakozik az egyik potenciometer
int potTu2 = 5;  // az 5-ik analog ture csatlakozik a masik potenciometer

int Rele = 2;        // a rele ki/bekapcsolasat vezerlo tu a 2-es
int kapcsBemTu = 4;  // a kapcsolo digitalis bemeneti tuje a 4-es
int kapcs = 0;       // valtozo a kapcsolo allapotanak olvasasahoz

int pot1erteke;  // valtozo, amely olvassa az egyik analog tu erteket
int pot2erteke;  // valtozo, amely olvassa a masik analog tu erteket

void setup()
{
  pinMode(Rele, OUTPUT);       // a rele tuje kimenetre lett most beallitva
  pinMode(kapcsBemTu, INPUT);  // kihirdeti a kapcsolot, mint egy bemenetet
  digitalWrite(kapcsBemTu,HIGH); // turn on internal pull-up on the inputPin
  Serial.begin(115200);        // hibakereseshez soros portot nyit
  szervo1.attach(9);           // a vizszintesen forgo szervot csatolja a 9-es labon a szervo1 objektumhoz
  szervo2.attach(10);          // a fuggolegesen forgo szervot csatolja a 10-es labon a szervo2 objektumhoz
} 

void loop()
{
  kapcs = digitalRead(kapcsBemTu);  // olvassa a kapcsolo bemenetenek erteket
  if (kapcs == HIGH)                // ellenorzi, hogy a bemenet MAGAS (HIGH) -e?
  {
    // *** program vezerli a szervokat es az elektromagnest
    szervo1.write(31);              // beallitja a szervo1 helyzetet (jobbra)
    delay (900);                    // var, hogy a szervo odaerjen
    szervo2.write(163);             // beallitja a szervo2 helyzetet (egeszen le)
    delay (900);                    // var, hogy a szervo odaerjen
    digitalWrite(Rele, HIGH);       // bekapcsolja a relevel az elektromagnest
    delay (2000);                   // var 2 masodpercig, hogy a magnes megfogjon egy targyat
    szervo2.write(42);              // beallitja a szervo2 helyzetet (föl)
    delay (1000);                   // var, hogy a szervo odaerjen
    szervo1.write(135);             // beallitja a szervo1 helyzetet (balra)
    delay (900);                    // var, hogy a szervo odaerjen
    szervo2.write(152);             // beallitja a szervo2 helyzetet (le)
    delay (900);                    // var, hogy a szervo odaerjen
    digitalWrite(Rele, LOW);        // kikapcsolja a relevel az elektromagnest
    delay (3000);                   // var 3 masodpercig
    szervo2.write(42);              // beallitja a szervo2 helyzetet (föl)
    delay (1000);                   // var, hogy a szervo odaerjen
  }                                 // ugrik a loop() (kör) elejere
  else
  {
    // *** potenciometerekkel vezerlem a szervokat, nincs elektromagnes
    pot1erteke = analogRead(potTu1);                // a potenciometer erteket olvassa (az ertek lehet 0-1023) 
    pot1erteke = map(pot1erteke, 0, 1023, 0, 179);  // aranyosan csokkenti, hogy megfeleljen a szervonak (az ertek lehet 0-180) 
    szervo1.write(pot1erteke);                      // beallitja a szervo helyzetet az aranyosan lecsokkentett erteknek megfeleloen
    Serial.print("szervo1 = ");                      // soros portra kiirja a szoveget "Val1= "
    Serial.println(pot1erteke);                     // soros portra kiirja az erteket: val1
    
    pot2erteke = analogRead(potTu2);                // a potenciometer erteket olvassa (az ertek lehet 0-1023) 
    pot2erteke = map(pot2erteke, 0, 1023, 0, 179);  // aranyosan csokkenti, hogy megfeleljen a szervonak (az ertek lehet 0-180) 
    szervo2.write(pot2erteke);                      // beallitja a szervo helyzetet az aranyosan lecsokkentett erteknek megfeleloen
    Serial.print("szervo2 = ");                      // soros portra kiirja a szoveget "Val2= "
    Serial.println(pot2erteke);                     // soros portra kiirja az erteket: val2
    delay(50);                                      // var, hogy a szervok odaerjenek
  }                                                 // ugrik a loop() (kör) elejere
}