![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Diff: verwerking/goto.cpp
- Revision:
- 0:bec310bde899
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/verwerking/goto.cpp Tue Sep 27 19:46:30 2011 +0000 @@ -0,0 +1,106 @@ +#include "goto.h" + + +Goto::Goto(Gps * g, Route * r, Kompas * k, Vaantje * v){ + huidig = g; + doel = r; + kompas = k; + vaantje = v; + + opkruisen = 0; + eerste = 1; + reset = 0; + rak = 0; + holdcourse = 0; + course = 0; + minhoek = 35; + hys = 5; + +} +void Goto::manual(int m, int k){ + holdcourse = m; + course = k; +} + + +double Goto::get(void){ + if (holdcourse) + return course; + + double k = koers(&huidig->get(),&doel->get()); //koers naar doel berekenen + double d = afstand(&huidig->get(),&doel->get()); //afstand nar doel berekenen + + + double ware_wind = vaantje->get(); //ware wind uit vaantje halen + + if (ware_wind > 360) + ware_wind -= 360; + + if (fabs(verschil(k,ware_wind)) < minhoek + hys){ //kijken of er moet worden opgekruist + if (!opkruisen){ //kijken of er niet al opgekruist wordt + begin_opkruisen = huidig->get(); //huidige positie opslaan als begin van het + // opkruisen + begin_rak = huidig->get(); //en als begin van het huidige rak + koers_begin = k; //de koers van de layline opslaan + eerste = 1; //installen dat het eerste rak is + } + else{ + reset = 0; + } + opkruisen = 1; + } + if (fabs(verschil(k,ware_wind)) > minhoek + hys){ //kijken of het al bezeild is + opkruisen = 0; + eerste = 0; + } + if (!opkruisen){ //als er niet opgekruist hoeft te worden + return k; //direct naar het doel varen + } + else //anders: + { + double boeg1 = ware_wind + (double)minhoek; //twee koersen uitrekenen + double boeg2 = ware_wind - (double)minhoek; + if (boeg1 > 360) + boeg1 -= 360; + if (boeg2 < 0) + boeg2 += 360; + + double lengte = afstand(&begin_opkruisen, &doel->get()) / 5.0; //lengte van de rakken + // uitrekenen + double a = fabs(verschil(ware_wind,koers_begin)); + double lengte1 = lengte / sin(((double)minhoek+a) * deg2rad); + double lengte2 = lengte / sin(((double)minhoek-a) * deg2rad); + + if (eerste){ + //eerste rak 2x zo kort + lengte1 /= 2.0; + lengte2 /= 2.0; + //bij eerste rak kiezen welke koers het dichste bij de huidige licht + if (fabs(verschil(boeg1,kompas->get())) < fabs(verschil(boeg2,kompas->get()))) + boeg = 1; + else + boeg = 2; + } + if (boeg == 1){ + if (afstand(&begin_rak,&huidig->get()) > lengte1){ //als het einde van het rak bereikt is + eerste = 0; + boeg = 2; //ga overstag + begin_rak = huidig->get(); + } + return boeg1; + } + if (boeg == 2){ + if (afstand(&begin_rak,&huidig->get()) > lengte2){ + eerste = 0; + boeg = 1; + begin_rak = huidig->get(); + } + return boeg2; + } + } +} + + + + + \ No newline at end of file