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