////// VENETIETO.FI \\\\\\ //// Autopilotti 2014 \\\\ //-PID säätimellä #include <EEPROM.h> #include <SoftwareSerial.h> SoftwareSerial gps(0, 1); // RX, TX -pinnit const int buttonpinset = 2; // painonäppäimen pinni const int buttonpinclear = 4; // painonäppäimen pinni const int buttonpinoikea = 12; // painonäppäimen pinni const int buttonpinvasen = 11; // painonäppäimen pinni const int pot = analogread(a1); const int ledpin = 13; // LED pinni const int Dir2a = 7; //PWM suunta 2a const int Dir2b = 8; //PWM suunta 2b const int Enable2 = 5; //PWM E2 const int numreadings = 5; int buttonstateset = 0; // muutuja pianonäppäimen tilan lukemiseksi int buttonstateclear = 0; // muutuja pianonäppäimen tilan lukemiseksi int buttonstateoikea = 0; int buttonstatevasen = 0; int ero = 0; // muutuja arvolle int address = 0; // muutuja arvolle int readings[numreadings]; // lukemat GPS:ltä int index = 0; int total = 0; // lukeman numero // kokonaismäärä
int average = 0; // keskiarvo int pidnykyarvo = 0; int ulostulo; int erosumma; unsigned int lastgpsread; const unsigned int gpscycle=1000; //1Hz GPS taajuus float gpslat0; float gpslong0; float gpslat; float gpslong; float gpsspeed; //solmua float gpsbearing; //suunta byte tallennus; void setup() Serial.begin(38400); gps.begin(38400); pinmode(ledpin, OUTPUT); //asettaa pinnin ulostuloksi pinmode(dir2a, OUTPUT); pinmode(dir2b, OUTPUT); pinmode(enable2, OUTPUT); pinmode(buttonpinset, INPUT); //asettaa pinnin sisääntuloksi pinmode(buttonpinclear, INPUT); pinmode(buttonpinoikea, INPUT); pinmode(buttonpinvasen, INPUT); for (int thisreading = 0; thisreading < numreadings; thisreading++) readings[thisreading] = 0;
lastgpsread=millis()-gpscycle; //Aloitetaan lukemalla GPS-data void loop() unsigned long now=millis(); if(now-lastgpsread >= (gpscycle-150)) //Jos on mennyt koko syklin aika, niin ollaan luultavasti jo myöhässä, aletaan odottamaan jo 150 ms ennen. if (gps.available() > 1) //lue arvo kun GPS antaa dataa else gpsohjaus(); delay(10); if (char(gps.read()) == 'R' && char(gps.read()) == 'M' && char(gps.read()) == 'C') //lue arvo jossa R,M ja C gps.parsefloat(); //poista turhat gpslatlong(gps.parseint(), gps.parseint(), gps.parseint(), gps.parseint()); //sijainti gpsohjaus(); //toista lauseke "gpsohjaus"gpsspeed = gps.parsefloat(); //solmua gpsbearing = gps.parsefloat(); //suunta lastgpsread=now; delay(10); // void gpslatlong(int lat1, int lat2, int long1, int long2) //paikan, nopeuden ja suunnan printti, jos vaikka haluaa tulevaisuudessa näytölle gpslat = int(lat1/100) + (lat1%100)/60.0 + float(lat2)/10000.0/60.0;
gpslong = int(long1/100) + (long1%100)/60.0 + float(long2)/10000.0/60.0; Serial.print(gpsLat,5); Serial.print("/"); Serial.println(gpsLong,5); Serial.print("Nopeus:"); Serial.println(gpsSpeed,3); Serial.print("Suunta:"); Serial.println(gpsBearing,2); void gpsohjaus() //ohjaustoiminto buttonstateset = digitalread(buttonpinset); buttonstateclear = digitalread(buttonpinclear); tallennus = EEPROM.read(0); //EEPROM muistin luku osoitteesta (0) //Keskiarvo GPS suunnasta total= total - readings[index]; readings[index] = gpsbearing; total= total + readings[index]; index = index + 1; average = total / 5; int suunta = average/1.42; //suuntima on 0-360, mutta EEPROM tallentaa arvot vain välillä 0-255 //PID parametrit char erotus = suunta-tallennus; //hetkellisen suunnan ja tallennuksen ero int ero = erotus;
int ulostulo; int pidedellinenarvo = pidnykyarvo; pidnykyarvo = suunta; //PID-säädin if (abs(ero) >= 2) ulostulo = 5*ero+1*erosumma+5*(pidNykyArvo-pidEdellinenArvo); // Kerroin on arvon vahvistus erosumma += ero; else ulostulo = 0; if (index >= 5) index = 0; int ohjaus = 120 + ulostulo; //120 potentiometrin keskikohta if (buttonstateset == HIGH) //toimi jos Set nappia painettu EEPROM.write(0, suunta); //kirjoita EEPROMn osoitteeseen (0) suuntiman arvo digitalwrite(13, HIGH); //sytytä LED if (buttonstateclear == HIGH) //toimi jos Reset nappia painettu EEPROM.write(0, 0); //kirjoita EEPROMn osoitteeseen (0) arvo 0 digitalwrite(13, LOW); //sammuta LED if (tallennus!= 0) //toimi jos tallennuksen:n arvo ei ole 0 if (pot > ohjaus && ohjaus > -100) digitalwrite(7, LOW);
digitalwrite(8, HIGH); analogwrite(5,255); else if (pot < ohjaus && ohjaus < 100) digitalwrite(7, HIGH); digitalwrite(8, LOW); analogwrite(5,255); else digitalwrite(7, LOW); digitalwrite(8, LOW); analogwrite(5,0); Serial.print("ero:"); //tulosta ero sarjaporttiin Serial.println(ero); Serial.print("average:"); //tulosta keskiarvo sarjaporttiin Serial.println(average); if (tallennus == 0) //toimi jos tallennuksen:n arvo on 0 if (buttonstateoikea == HIGH && 20 < pot < 200) digitalwrite(7, HIGH); digitalwrite(8, LOW); analogwrite(5,180); else if (buttonstatevasen == HIGH && 20 < pot < 200) digitalwrite(7, LOW);
digitalwrite(8, HIGH); analogwrite(5,180); else analogwrite(5,0); Serial.print("tallennus:"); //tulosta tallennus sarjaporttiin Serial.println(tallennus);