Diff: verwerking/goto.cpp

Revision:

0:bec310bde899

diff -r 000000000000 -r bec310bde899 verwerking/goto.cpp
--- /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