File content as of revision 0:bec310bde899:

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