//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;
} |