Koodi Arduinolla -Code with Arduino

//Kirjastot
#include <Servo.h>
//Servo-objektien alustus
Servo servoSuuntaVasen;
Servo servoSuuntaOikea;
Servo servoTehoVasen;
Servo servoTehoOikea;
//Vakio Muuttujat
const float degConversion = 57.2957795; //radiaani asteina
const float roundingNumber = 0.5; // pyoristys
//Trimmaus Muuttujat
//Näiden muuttujien arvoa voi muuttaa kun halutaan optimoida erilaisia asetuksia
const int trimmausKeskiAsennonKlappi = 2; //voidaan asettaa pieni välysliike tatin keskiasentoon jolloin moottorit eivät vielä vastaa
const int trimmausTehoSivuttaisliike = 10; //käytetään siihen, että saadaan trimmattua sivuttaisliike tattia vastaavaksi
const int trimmausMoottorinKaantoNopeus = 10; //suurempi luku tarkoittaa suurempaa maksimi kääntymisnopeutta. Arvo välillä 0 - 90
const long trimmausMoottorinKiihtymisNopeus = 10; //suurempi luku hidastaa moottoreiden tehon muutosta (arvo millisekuntia per aste)
const int trimmausZvalys = 2; //voidaan asettaa tyhjä alue joystickin kiertoliikkeelle
const int moottorinMaksimiKaantoAjossa = 10; //asetetaan maksimi kääntökulma moottoreille ajossa. Arvo annetaan asteina 0 - 30
const int rajaTehonMuutokseen = 15; //suurempi arvo tarkoittaa, että tatin täytyy olla tämän rajan yläpuolella tietyn aikaa jos halutaan muuttaa nopeutta fishing modessa. arvo on välillä 0 - 22, mutta mielummin isompi kuin nolla
const long tehonMuutoksenSykli = 200; //yksikössä millisekuntia [ms]. voidaan muuttaa nopeuden muutoksen sykliä fishing modessa
const long kytkimenHyppyAika = 200; //jos painonapit "vilkkuu" ei kytkeydy kunnolla, voi tätä arvoa yrittää hieman kasvattaa. Arvo on [ms] millisekunteina
const long vilkkuvanVihreanTaajuus = 1000; //yksikössä millisekuntia [ms]. Voidaan säätää nopeutta jolla välkyntä tapahtuu
int moottorinMaksimiNopeusSteering = 90; //vaikuttaa moottoreiden maksiminopeuteen steering modessa. Arvo välillä 0 - 90
//Yleiset Muuttujat Alustus
//Moottoreiden asennot
boolean kotiKytkimenArvoVasen = 0;
boolean kotiKytkimenArvoOikea = 0;
volatile int suuntaPulssitVasen = 0;
volatile int suuntaPulssitOikea = 0;
volatile boolean edellinenVasen = 0;
volatile boolean paivitaVasen = false;
volatile boolean edellinenOikea = 0;
volatile boolean paivitaOikea = false;
//Tehon laskenta
int nykyinenTehoVasen = 90;
int nykyinenTehoOikea = 90;
int teho = 0;
int tehoProsentteina = 0;
float tehokerroin = 0;
int moottorinTehoVasen = 0;
int moottorinTehoOikea = 0;
//Ajastetut toiminnot
unsigned long aika = 0;
unsigned long aikaMuutoksestaVihrea = 0;
unsigned long aikaTehonMuutoksesta = 0;
unsigned long aikaTehonMuutoksesta2 = 0;
boolean tilaPunainen = false;
boolean tilaVihrea = false;
boolean edellinenPunainen = false;
boolean edellinenVihrea = false;
int valonTilaVihrea = LOW;
boolean tehoJoMuutettuSyklilla = false;
boolean tehonMuutos = false;
boolean tehoJoMuutettuSyklilla2 = false;
boolean tehonMuutos2 = false;
//Joystick
int joyXmap = 0;
int joyYmap = 0;
int joyZmap = 0;
int absoluteX = 0;
int absoluteY = 0;
int absoluteZ = 0;
//Moottoreiden asentojen laskentaan
float kulmakerroin = 0;
float kulmaRadiaaneina = 0;
int kulma = 0;
int kulmakerroinProsentteina = 0;
int moottorinAsento =0;
int rajaPositiivinen = 0;
int rajaNegatiivinen = 0;
int moottoreidenRajaAsennotAjossa = 0;
int moottorinAsentoVasen = 0;
int moottorinAsentoOikea = 0;
int moottorinOhjausVasen = 0;
int moottorinOhjausOikea = 0;
//Ajomoodin valinta
int lukemaNappiPunainen = false;
int lukemaNappiVihrea = false;
int ajoMoodi = 0;
//Trimmausten laskemiseen
int trimmikulmat = 0;
int trimmikulmaProsentteina = 0;
float trimmiKulmakerroin = 0;
int trimmiTeho = 0;
//Pinnit
const int joyX = A0;
const int joyY = A1;
const int joyZ = A2;
const int kotiPinVasen = 4;
const int suuntaYksiPinVasen = 2; //muutos
const int suuntaKaksiPinVasen = 8;
const int kotiPinOikea = 11;
const int suuntaYksiPinOikea = 3; //muutos
const int suuntaKaksiPinOikea = 1;
const int ledPunainen = 13;
const int ledVihrea = 0;
const int nappiPunainen = 7; //muutos
const int nappiVihrea = 12; //muutos
void setup() //laudan asetukset
{
//Serial.begin(9600); //sarjaliikenteen käynnistys nopeudella 9600bps
  
  //Pinnien tilat
  pinMode(kotiPinVasen, INPUT);
  pinMode(suuntaYksiPinVasen, INPUT);
  pinMode(suuntaKaksiPinVasen, INPUT);
  pinMode(kotiPinOikea, INPUT);
  pinMode(suuntaYksiPinOikea, INPUT);
  pinMode(suuntaKaksiPinOikea, INPUT);
  pinMode(nappiPunainen, INPUT);
  pinMode(nappiVihrea, INPUT);
  pinMode(ledPunainen, OUTPUT);
  pinMode(ledVihrea, OUTPUT);
  
  servoSuuntaVasen.attach(5, 1000, 2000); //ykkos moottorin ohjaus, pinni 5 PWM
  servoSuuntaOikea.attach(6, 1000, 2000); //kakkos moottorin ohjaus, pinni 6 PWM
  servoTehoVasen.attach(9, 1000, 2000);
  servoTehoOikea.attach(10, 1000, 2000);
  
  ajaKotiin(); //Ajetaan moottorin 0 kohtaan
//  ajaKotiinOikea();
  attachInterrupt(0, pulssilaskuriVasen, FALLING);
  attachInterrupt(1, pulssilaskuriOikea, FALLING);
}
void loop() //pääohjelman kierto alkaa
{
//  Serial.print("asento vasen moottori  ");
//  Serial.print(moottorinAsentoVasen);
//  Serial.print("\t asento oikea moottori  ");
//  Serial.print(moottorinAsentoOikea);
//  Serial.print("pulssit vasen moottori  ");
//  Serial.print(suuntaPulssitVasen);
//  Serial.print("\t pulssit oikea moottori  ");
//  Serial.print(suuntaPulssitOikea);
//  Serial.print("\t X ");
//  Serial.print(joyXmap);
//  Serial.print("\t Y ");
//  Serial.print(joyYmap);
//  Serial.print("\t Z ");
//  Serial.println(joyZmap);
//Pulssilaskurit
  if(paivitaVasen){
    paivitaVasen = false;
    if (edellinenVasen == true)
      suuntaPulssitVasen++;
    else
      suuntaPulssitVasen--;
  }
  if(paivitaOikea){
    paivitaOikea = false;
    if (edellinenOikea == true)
      suuntaPulssitOikea++;
    else
      suuntaPulssitOikea--;
  }
  
//Lutetaan mikrokytkimet
  lukemaNappiPunainen = digitalRead(nappiPunainen);
  lukemaNappiVihrea = digitalRead(nappiVihrea);
//Vaihdetaan kytkimien tilaa
  if(lukemaNappiPunainen == HIGH && edellinenPunainen == false && millis() - aika > kytkimenHyppyAika) {
   if (tilaPunainen == true)
    tilaPunainen = false; 
  else
    tilaPunainen = true;
  aika = millis();
  tilaVihrea = false;
  }
  if(lukemaNappiVihrea == HIGH && edellinenVihrea == false && millis() - aika > kytkimenHyppyAika) {
    if (tilaVihrea == true)
      tilaVihrea = false;
    else
      tilaVihrea = true;
    aika = millis();
    tilaPunainen = false;
  }
  edellinenPunainen = lukemaNappiPunainen;
  edellinenVihrea = lukemaNappiVihrea;
  
//Kirjoitetaan tila ajo moodiin
  if(tilaPunainen == true) 
    ajoMoodi = 1;
  
  if(ajoMoodi == 1 && tilaPunainen == false) 
    ajoMoodi = 0;
  
  if(tilaVihrea == true) 
    ajoMoodi = 2;
  
  if(ajoMoodi == 2 && tilaVihrea == false) 
    ajoMoodi = 3;
  
//Luetaan Joystick
  joyXmap = map(analogRead(joyX), 0, 1023, -22, 22);
  joyXmap = constrain(joyXmap, -22, 22);
  joyYmap = map(analogRead(joyY), 0, 1023, -22, 22);
  joyYmap = constrain(joyYmap, -22, 22);
  joyZmap = map(analogRead(joyZ), 0, 1023, -22, 22);
  joyZmap = constrain(joyZmap, -22, 22);
  absoluteX = abs(joyXmap); //Joystick X arvon itseisarvo
  absoluteY = abs(joyYmap); //Joystick Y arvon itseisarvo
  absoluteZ = abs(joyZmap); //Joystick Z arvon itseisarvo
//Ajomoodin valinta
switch (ajoMoodi){
  
  case 0: //Offline mode kytketty
    digitalWrite(ledPunainen, LOW);
    digitalWrite(ledVihrea, LOW);
    moottorinTehoVasen = 90;
    moottorinTehoOikea = 90;
    moottorinAsentoVasen = 22;
    moottorinAsentoOikea = 22;
    
  break;
  
  case 1: //Steering mode kytketty
    digitalWrite(ledPunainen, HIGH);
    digitalWrite(ledVihrea, LOW);
    moottorinMaksimiNopeusSteering =constrain(moottorinMaksimiNopeusSteering, 0, 90);
    if (absoluteZ > 0 + trimmausZvalys) { //Akselin ympäri kääntyminen
      moottorinAsentoVasen = 22;
      moottorinAsentoOikea = 22;
      tehoProsentteina =map(absoluteZ, 0, 22, 0, 100);
      tehoProsentteina =constrain(tehoProsentteina, 0, 100);
      tehokerroin = tehoProsentteina * 0.01;
      if (joyZmap > 0) {
        moottorinTehoVasen = 90 - 90 * tehokerroin + roundingNumber;
        moottorinTehoOikea = 90 + 90 * tehokerroin + roundingNumber;
      }
      else {
        moottorinTehoVasen = 90 + 90 * tehokerroin + roundingNumber;
        moottorinTehoOikea = 90 - 90 * tehokerroin + roundingNumber;        
      }
    }
    else { //Ohjaus sivusuunnissa
      teho = sqrt(sq(absoluteX)+sq(absoluteY))+roundingNumber;
      tehoProsentteina =map(teho, 0, 22, 0, 100);
      tehoProsentteina =constrain(tehoProsentteina, 0, 100);
      tehokerroin = tehoProsentteina * 0.01;
    
      if (absoluteY >= absoluteX) {
      kulmaRadiaaneina = atan2(absoluteX ,absoluteY); //arcustangentti radiaaneina
      }
      if (absoluteX >= absoluteY) {
      kulmaRadiaaneina = atan2(absoluteY ,absoluteX); //arcustangentti radiaaneina
      }
      
      kulma = kulmaRadiaaneina * degConversion + roundingNumber;
      kulmakerroinProsentteina = map(kulma, 0, 45, 0, 100);
      kulmakerroin = kulmakerroinProsentteina * 0.01;
      trimmikulmat = constrain(kulma, 30, 45);
      trimmikulmaProsentteina = map(trimmikulmat, 30, 45, 0, 100);
      trimmikulmaProsentteina = constrain(trimmikulmaProsentteina, 0, 100);
      trimmiKulmakerroin = trimmikulmaProsentteina * 0.01;
      trimmiTeho = trimmausTehoSivuttaisliike * tehokerroin * trimmiKulmakerroin;
    
      if ((absoluteX <= absoluteY) && (joyXmap >= rajaPositiivinen) && (joyYmap >= rajaPositiivinen)) { // 0- 45 astetta
        moottorinAsentoVasen = 22 + 22 * kulmakerroin + roundingNumber;
        moottorinAsentoOikea = 22;
        moottorinTehoVasen = 90 + moottorinMaksimiNopeusSteering * tehokerroin;
        moottorinTehoOikea = (90-trimmiTeho) + tehokerroin * (1-kulmakerroin) * moottorinMaksimiNopeusSteering + roundingNumber;
      }
      
      if ((absoluteX >= absoluteY)  && (joyXmap >= rajaPositiivinen) && (joyYmap >= rajaPositiivinen)) { // 45- 90 astetta
        moottorinAsentoVasen = 44;
        moottorinAsentoOikea = 22 - 22 * (1-kulmakerroin) + roundingNumber;
        moottorinTehoVasen = 90 + moottorinMaksimiNopeusSteering * tehokerroin;
        moottorinTehoOikea = (90-trimmiTeho) - tehokerroin * (1-kulmakerroin) * moottorinMaksimiNopeusSteering + roundingNumber;
      }
      
      if ((absoluteX >= absoluteY) && (joyXmap >= rajaPositiivinen) && (joyYmap <= rajaNegatiivinen)) { // 90- 135 astetta
        moottorinAsentoVasen = 22 + 22 * (1-kulmakerroin) + roundingNumber;
        moottorinAsentoOikea = 0;
        moottorinTehoVasen = (90+trimmiTeho) + tehokerroin * (1-kulmakerroin) * moottorinMaksimiNopeusSteering + roundingNumber;
        moottorinTehoOikea = 90 - moottorinMaksimiNopeusSteering * tehokerroin;
      }
      
      if ((absoluteX <= absoluteY)  && (joyXmap >= rajaPositiivinen) && (joyYmap <= rajaNegatiivinen)) { // 135- 180 astetta
        moottorinAsentoVasen = 22;
        moottorinAsentoOikea = 22 - 22 * kulmakerroin + roundingNumber;
        moottorinTehoVasen = (90+trimmiTeho) - tehokerroin * (1-kulmakerroin) * moottorinMaksimiNopeusSteering + roundingNumber;
        moottorinTehoOikea = 90 - moottorinMaksimiNopeusSteering * tehokerroin;
      }
      
      if ((absoluteX <= absoluteY) && (joyXmap <= rajaNegatiivinen) && (joyYmap <= rajaNegatiivinen)) { // 180- 225 astetta
        moottorinAsentoVasen = 22 + 22 * kulmakerroin + roundingNumber;
        moottorinAsentoOikea = 22;
        moottorinTehoVasen = 90 - moottorinMaksimiNopeusSteering * tehokerroin;
        moottorinTehoOikea = (90+trimmiTeho) - tehokerroin * (1-kulmakerroin) * moottorinMaksimiNopeusSteering + roundingNumber;
      }
      
      if ((absoluteX >= absoluteY)  && (joyXmap <= rajaNegatiivinen) && (joyYmap <= rajaNegatiivinen)) { // 225- 270 astetta
        moottorinAsentoVasen = 44;
        moottorinAsentoOikea = 22 - 22 * kulmakerroin + roundingNumber;
        moottorinTehoVasen = 90 - moottorinMaksimiNopeusSteering * tehokerroin;
        moottorinTehoOikea = (90+trimmiTeho) + tehokerroin * (1-kulmakerroin) * moottorinMaksimiNopeusSteering + roundingNumber;
      }
      
      if ((absoluteX >= absoluteY) && (joyXmap <= rajaNegatiivinen) && (joyYmap >= rajaPositiivinen)) { // 270- 315 astetta
        moottorinAsentoVasen = 22 + 22 * (1-kulmakerroin) + roundingNumber;
        moottorinAsentoOikea = 0;
        moottorinTehoVasen = (90-trimmiTeho) - tehokerroin * (1-kulmakerroin) * moottorinMaksimiNopeusSteering + roundingNumber;
        moottorinTehoOikea = 90 + moottorinMaksimiNopeusSteering * tehokerroin;
      }
      
      if ((absoluteX <= absoluteY)  && (joyXmap <= rajaNegatiivinen) && (joyYmap >= rajaPositiivinen)) { // 315- 0 astetta
        moottorinAsentoVasen = 22;
        moottorinAsentoOikea = 22 - 22 * kulmakerroin + roundingNumber;
        moottorinTehoVasen = (90-trimmiTeho) + tehokerroin * (1-kulmakerroin) * moottorinMaksimiNopeusSteering + roundingNumber;
        moottorinTehoOikea = 90 + moottorinMaksimiNopeusSteering * tehokerroin;
      }
      if (absoluteY <= trimmausKeskiAsennonKlappi && absoluteX <= trimmausKeskiAsennonKlappi) {
        moottorinTehoVasen = 90;
        moottorinTehoOikea = 90;
        moottorinAsentoVasen = 22;
        moottorinAsentoOikea = 22;
      }
    }
    break;
  
  case 2: //Drive moodi kytketty
      digitalWrite(ledPunainen, LOW);
      digitalWrite(ledVihrea, HIGH);
      moottoreidenRajaAsennotAjossa = map(moottorinMaksimiKaantoAjossa, 0, 30, 0, 22);
      moottoreidenRajaAsennotAjossa = constrain(moottoreidenRajaAsennotAjossa, 0, 22);
      
      if (absoluteY > trimmausKeskiAsennonKlappi) {
        tehoProsentteina =map(absoluteY, 0, 22, 0, 100);
        tehoProsentteina =constrain(tehoProsentteina, 0, 100);
        tehokerroin = tehoProsentteina * 0.01;
        if (joyYmap > 0) {
          moottorinTehoVasen = 90 + 90 * tehokerroin + roundingNumber;
          moottorinTehoOikea = moottorinTehoVasen;
        }
        else {
          moottorinTehoVasen = 90 - 90 * tehokerroin + roundingNumber;
          moottorinTehoOikea = moottorinTehoVasen;
        }
      }
        else {
          moottorinTehoVasen = 90;
          moottorinTehoOikea = 90;
        }
      if (absoluteX > trimmausKeskiAsennonKlappi) {
        kulmakerroinProsentteina =map(absoluteX, 0, 22, 0, 100);
        kulmakerroinProsentteina =constrain(kulmakerroinProsentteina, 0, 100);
        kulmakerroin = kulmakerroinProsentteina * 0.01;
        if (joyXmap > 0) {
          moottorinAsentoVasen = 22 + moottoreidenRajaAsennotAjossa * kulmakerroin + roundingNumber;
          moottorinAsentoOikea = moottorinAsentoVasen;
        }
        else {
          moottorinAsentoVasen = 22 - moottoreidenRajaAsennotAjossa * kulmakerroin + roundingNumber;
          moottorinAsentoOikea = moottorinAsentoVasen;
    }
  }
    else {
      moottorinAsentoVasen = 22;
      moottorinAsentoOikea = 22;
    }
    
    break;
  
  case 3: //Fishing moodi kytketty
    //Vihreä valo vilkkuu syklillä jonka pituus on: "vilkkuvanVihreanTaajuus"
    if(millis() - aikaMuutoksestaVihrea > vilkkuvanVihreanTaajuus) {
      aikaMuutoksestaVihrea = millis();
      
      if (valonTilaVihrea == LOW)
        valonTilaVihrea = HIGH;
      else
        valonTilaVihrea = LOW;
    }
    digitalWrite(ledVihrea, valonTilaVihrea);
    
    if (absoluteY > rajaTehonMuutokseen){
    //Pulssi kytkin jonka pulssin aika riippuu muuttujasta: "tehonMuutoksenSykli"
      if(millis() - aikaTehonMuutoksesta > tehonMuutoksenSykli) {
        aikaTehonMuutoksesta = millis();
        
        if (tehonMuutos == false) 
          tehonMuutos = true;
        else {
            tehonMuutos = false;
            tehoJoMuutettuSyklilla = false;
        }
      }
      //Muuttaa moottoreiden tehoa jos pulssi on positiivinen ja sitä ei ole vielä muutettu positiivisella alueella
      if (tehonMuutos == true && tehoJoMuutettuSyklilla == false) {
        if (joyYmap > 0) {
          moottorinTehoVasen = moottorinTehoVasen ++;
          moottorinTehoOikea = moottorinTehoOikea ++;
        }
        if (joyYmap < 0) {
          moottorinTehoVasen = moottorinTehoVasen --;
          moottorinTehoOikea = moottorinTehoOikea --;
        }
        tehoJoMuutettuSyklilla = true;
      }
    }
    if (absoluteX > trimmausKeskiAsennonKlappi) {
      kulmakerroinProsentteina =map(absoluteX, 0, 22, 0, 100);
      kulmakerroinProsentteina =constrain(kulmakerroinProsentteina, 0, 100);
      kulmakerroin = kulmakerroinProsentteina * 0.01;
      if (joyXmap > 0) {
        moottorinAsentoVasen = 22 + moottoreidenRajaAsennotAjossa * kulmakerroin + roundingNumber;
        moottorinAsentoOikea = moottorinAsentoVasen;
      }
      else {
        moottorinAsentoVasen = 22 - moottoreidenRajaAsennotAjossa * kulmakerroin + roundingNumber;
        moottorinAsentoOikea = moottorinAsentoVasen;
      }
    }
    if (absoluteX <= trimmausKeskiAsennonKlappi) {
      moottorinAsentoVasen = 22;
      moottorinAsentoOikea = 22;
    }
    
    break;
  }
  //moottoreiden fyysiset liikkeet
  if(suuntaPulssitVasen == moottorinAsentoVasen)
    moottorinOhjausVasen = 90;
  else if(suuntaPulssitVasen < moottorinAsentoVasen)
    moottorinOhjausVasen = 90-trimmausMoottorinKaantoNopeus;
  else
    moottorinOhjausVasen = 90+trimmausMoottorinKaantoNopeus;
  if(suuntaPulssitOikea == moottorinAsentoOikea)
    moottorinOhjausOikea = 90;
  else if (suuntaPulssitOikea < moottorinAsentoOikea)
    moottorinOhjausOikea = 90-trimmausMoottorinKaantoNopeus;
  else
    moottorinOhjausOikea = 90+trimmausMoottorinKaantoNopeus;
    
  if(millis() - aikaTehonMuutoksesta2 > trimmausMoottorinKiihtymisNopeus) {
        aikaTehonMuutoksesta2 = millis();
        
        if (tehonMuutos2 == false) 
          tehonMuutos2 = true;
        else {
            tehonMuutos2 = false;
            tehoJoMuutettuSyklilla2 = false;
        }
      }
  if (tehonMuutos2 == true && tehoJoMuutettuSyklilla2 == false) {
    if (nykyinenTehoVasen != moottorinTehoVasen || nykyinenTehoOikea != moottorinTehoOikea){
    if (nykyinenTehoVasen < moottorinTehoVasen)
      nykyinenTehoVasen++;
    else
      nykyinenTehoVasen--;
    if (nykyinenTehoOikea < moottorinTehoOikea)
      nykyinenTehoOikea++;
    else
      nykyinenTehoOikea--;
    moottorinOhjausVasen = constrain(moottorinOhjausVasen, 0, 180);
    moottorinOhjausOikea = constrain(moottorinOhjausOikea, 0, 180);
    nykyinenTehoVasen = constrain(nykyinenTehoVasen, 0, 180);
    nykyinenTehoOikea = constrain(nykyinenTehoOikea, 0, 180);
    
    servoSuuntaVasen.write(moottorinOhjausVasen);
    servoSuuntaOikea.write(moottorinOhjausOikea);
    servoTehoVasen.write(nykyinenTehoVasen);
    servoTehoOikea.write(nykyinenTehoOikea);
    }
    tehoJoMuutettuSyklilla2 = true;
  }
}
void ajaKotiin()
{
  //Vasen Moottori
  kotiKytkimenArvoVasen = digitalRead(kotiPinVasen);  //Luetaan koti-kytkimen arvo
  if(kotiKytkimenArvoVasen) //Onko kotikytkin kytketty?
  {
    moottorinOhjausVasen = 90-trimmausMoottorinKaantoNopeus;
  }
  else
  {
    moottorinOhjausVasen = 90+trimmausMoottorinKaantoNopeus;
  }
  servoSuuntaVasen.write(moottorinOhjausVasen);
  boolean tempVas = digitalRead(kotiPinVasen); //Luetaan koti-kytkimen arvo
  while(tempVas == kotiKytkimenArvoVasen) //odotetaan että kotikytkimen arvo muuttu
  {
    tempVas = digitalRead(kotiPinVasen); //Luetaan koti-kytkimen arvo
  }
//  Serial.println("Vasemman moottorin kotikytkin saavutettu.");
  //Moottorin pysäytys
  moottorinOhjausVasen = 90;
  servoSuuntaVasen.write(moottorinOhjausVasen);
  //Alustetaan keskikohta muuttujaan, 22 pulssia per puoli.
  suuntaPulssitVasen = 22; 
//void ajaKotiinOikea() 
  //Oikea Moottori
  kotiKytkimenArvoOikea = digitalRead(kotiPinOikea);
  if(kotiKytkimenArvoOikea)
  {
    moottorinOhjausOikea = 90-trimmausMoottorinKaantoNopeus;
  }
  else
  {
    moottorinOhjausOikea = 90+trimmausMoottorinKaantoNopeus;
  }
  servoSuuntaOikea.write(moottorinOhjausOikea);
  boolean tempOik = digitalRead(kotiPinOikea);
  while(tempOik == kotiKytkimenArvoOikea)
  {
    tempOik = digitalRead(kotiPinOikea);
  }
//  Serial.println("Oikean moottorin kotikytkin saavutettu.");
  moottorinOhjausOikea = 90;
  servoSuuntaOikea.write(moottorinOhjausOikea);
  suuntaPulssitOikea = 22;
}
void pulssilaskuriVasen()
{
  edellinenVasen=(boolean)digitalRead(suuntaKaksiPinVasen);
  paivitaVasen = true;
}
void pulssilaskuriOikea()
{
  edellinenOikea=(boolean)digitalRead(suuntaKaksiPinOikea);
  paivitaOikea = true;
}