Willem Melching
verwerking/goto.cpp@0:bec310bde899, 2011-09-27 (annotated)
- Committer:
- pd0wm
- Date:
- Tue Sep 27 19:46:30 2011 +0000
- Revision:
- 0:bec310bde899
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| pd0wm | 0:bec310bde899 | 1 | #include "goto.h" |
| pd0wm | 0:bec310bde899 | 2 | |
| pd0wm | 0:bec310bde899 | 3 | |
| pd0wm | 0:bec310bde899 | 4 | Goto::Goto(Gps * g, Route * r, Kompas * k, Vaantje * v){ |
| pd0wm | 0:bec310bde899 | 5 | huidig = g; |
| pd0wm | 0:bec310bde899 | 6 | doel = r; |
| pd0wm | 0:bec310bde899 | 7 | kompas = k; |
| pd0wm | 0:bec310bde899 | 8 | vaantje = v; |
| pd0wm | 0:bec310bde899 | 9 | |
| pd0wm | 0:bec310bde899 | 10 | opkruisen = 0; |
| pd0wm | 0:bec310bde899 | 11 | eerste = 1; |
| pd0wm | 0:bec310bde899 | 12 | reset = 0; |
| pd0wm | 0:bec310bde899 | 13 | rak = 0; |
| pd0wm | 0:bec310bde899 | 14 | holdcourse = 0; |
| pd0wm | 0:bec310bde899 | 15 | course = 0; |
| pd0wm | 0:bec310bde899 | 16 | minhoek = 35; |
| pd0wm | 0:bec310bde899 | 17 | hys = 5; |
| pd0wm | 0:bec310bde899 | 18 | |
| pd0wm | 0:bec310bde899 | 19 | } |
| pd0wm | 0:bec310bde899 | 20 | void Goto::manual(int m, int k){ |
| pd0wm | 0:bec310bde899 | 21 | holdcourse = m; |
| pd0wm | 0:bec310bde899 | 22 | course = k; |
| pd0wm | 0:bec310bde899 | 23 | } |
| pd0wm | 0:bec310bde899 | 24 | |
| pd0wm | 0:bec310bde899 | 25 | |
| pd0wm | 0:bec310bde899 | 26 | double Goto::get(void){ |
| pd0wm | 0:bec310bde899 | 27 | if (holdcourse) |
| pd0wm | 0:bec310bde899 | 28 | return course; |
| pd0wm | 0:bec310bde899 | 29 | |
| pd0wm | 0:bec310bde899 | 30 | double k = koers(&huidig->get(),&doel->get()); //koers naar doel berekenen |
| pd0wm | 0:bec310bde899 | 31 | double d = afstand(&huidig->get(),&doel->get()); //afstand nar doel berekenen |
| pd0wm | 0:bec310bde899 | 32 | |
| pd0wm | 0:bec310bde899 | 33 | |
| pd0wm | 0:bec310bde899 | 34 | double ware_wind = vaantje->get(); //ware wind uit vaantje halen |
| pd0wm | 0:bec310bde899 | 35 | |
| pd0wm | 0:bec310bde899 | 36 | if (ware_wind > 360) |
| pd0wm | 0:bec310bde899 | 37 | ware_wind -= 360; |
| pd0wm | 0:bec310bde899 | 38 | |
| pd0wm | 0:bec310bde899 | 39 | if (fabs(verschil(k,ware_wind)) < minhoek + hys){ //kijken of er moet worden opgekruist |
| pd0wm | 0:bec310bde899 | 40 | if (!opkruisen){ //kijken of er niet al opgekruist wordt |
| pd0wm | 0:bec310bde899 | 41 | begin_opkruisen = huidig->get(); //huidige positie opslaan als begin van het |
| pd0wm | 0:bec310bde899 | 42 | // opkruisen |
| pd0wm | 0:bec310bde899 | 43 | begin_rak = huidig->get(); //en als begin van het huidige rak |
| pd0wm | 0:bec310bde899 | 44 | koers_begin = k; //de koers van de layline opslaan |
| pd0wm | 0:bec310bde899 | 45 | eerste = 1; //installen dat het eerste rak is |
| pd0wm | 0:bec310bde899 | 46 | } |
| pd0wm | 0:bec310bde899 | 47 | else{ |
| pd0wm | 0:bec310bde899 | 48 | reset = 0; |
| pd0wm | 0:bec310bde899 | 49 | } |
| pd0wm | 0:bec310bde899 | 50 | opkruisen = 1; |
| pd0wm | 0:bec310bde899 | 51 | } |
| pd0wm | 0:bec310bde899 | 52 | if (fabs(verschil(k,ware_wind)) > minhoek + hys){ //kijken of het al bezeild is |
| pd0wm | 0:bec310bde899 | 53 | opkruisen = 0; |
| pd0wm | 0:bec310bde899 | 54 | eerste = 0; |
| pd0wm | 0:bec310bde899 | 55 | } |
| pd0wm | 0:bec310bde899 | 56 | if (!opkruisen){ //als er niet opgekruist hoeft te worden |
| pd0wm | 0:bec310bde899 | 57 | return k; //direct naar het doel varen |
| pd0wm | 0:bec310bde899 | 58 | } |
| pd0wm | 0:bec310bde899 | 59 | else //anders: |
| pd0wm | 0:bec310bde899 | 60 | { |
| pd0wm | 0:bec310bde899 | 61 | double boeg1 = ware_wind + (double)minhoek; //twee koersen uitrekenen |
| pd0wm | 0:bec310bde899 | 62 | double boeg2 = ware_wind - (double)minhoek; |
| pd0wm | 0:bec310bde899 | 63 | if (boeg1 > 360) |
| pd0wm | 0:bec310bde899 | 64 | boeg1 -= 360; |
| pd0wm | 0:bec310bde899 | 65 | if (boeg2 < 0) |
| pd0wm | 0:bec310bde899 | 66 | boeg2 += 360; |
| pd0wm | 0:bec310bde899 | 67 | |
| pd0wm | 0:bec310bde899 | 68 | double lengte = afstand(&begin_opkruisen, &doel->get()) / 5.0; //lengte van de rakken |
| pd0wm | 0:bec310bde899 | 69 | // uitrekenen |
| pd0wm | 0:bec310bde899 | 70 | double a = fabs(verschil(ware_wind,koers_begin)); |
| pd0wm | 0:bec310bde899 | 71 | double lengte1 = lengte / sin(((double)minhoek+a) * deg2rad); |
| pd0wm | 0:bec310bde899 | 72 | double lengte2 = lengte / sin(((double)minhoek-a) * deg2rad); |
| pd0wm | 0:bec310bde899 | 73 | |
| pd0wm | 0:bec310bde899 | 74 | if (eerste){ |
| pd0wm | 0:bec310bde899 | 75 | //eerste rak 2x zo kort |
| pd0wm | 0:bec310bde899 | 76 | lengte1 /= 2.0; |
| pd0wm | 0:bec310bde899 | 77 | lengte2 /= 2.0; |
| pd0wm | 0:bec310bde899 | 78 | //bij eerste rak kiezen welke koers het dichste bij de huidige licht |
| pd0wm | 0:bec310bde899 | 79 | if (fabs(verschil(boeg1,kompas->get())) < fabs(verschil(boeg2,kompas->get()))) |
| pd0wm | 0:bec310bde899 | 80 | boeg = 1; |
| pd0wm | 0:bec310bde899 | 81 | else |
| pd0wm | 0:bec310bde899 | 82 | boeg = 2; |
| pd0wm | 0:bec310bde899 | 83 | } |
| pd0wm | 0:bec310bde899 | 84 | if (boeg == 1){ |
| pd0wm | 0:bec310bde899 | 85 | if (afstand(&begin_rak,&huidig->get()) > lengte1){ //als het einde van het rak bereikt is |
| pd0wm | 0:bec310bde899 | 86 | eerste = 0; |
| pd0wm | 0:bec310bde899 | 87 | boeg = 2; //ga overstag |
| pd0wm | 0:bec310bde899 | 88 | begin_rak = huidig->get(); |
| pd0wm | 0:bec310bde899 | 89 | } |
| pd0wm | 0:bec310bde899 | 90 | return boeg1; |
| pd0wm | 0:bec310bde899 | 91 | } |
| pd0wm | 0:bec310bde899 | 92 | if (boeg == 2){ |
| pd0wm | 0:bec310bde899 | 93 | if (afstand(&begin_rak,&huidig->get()) > lengte2){ |
| pd0wm | 0:bec310bde899 | 94 | eerste = 0; |
| pd0wm | 0:bec310bde899 | 95 | boeg = 1; |
| pd0wm | 0:bec310bde899 | 96 | begin_rak = huidig->get(); |
| pd0wm | 0:bec310bde899 | 97 | } |
| pd0wm | 0:bec310bde899 | 98 | return boeg2; |
| pd0wm | 0:bec310bde899 | 99 | } |
| pd0wm | 0:bec310bde899 | 100 | } |
| pd0wm | 0:bec310bde899 | 101 | } |
| pd0wm | 0:bec310bde899 | 102 | |
| pd0wm | 0:bec310bde899 | 103 | |
| pd0wm | 0:bec310bde899 | 104 | |
| pd0wm | 0:bec310bde899 | 105 | |
| pd0wm | 0:bec310bde899 | 106 |