Problem Sinus durch eigene Funktion ersetzen

[damfino]

Es wird damit der Weg der Odometrie berechnet:

#include <stdlib.h>
#include <math.h>
#include <karte.h>
#include <position_odometrie.h>
#include <allgemeine_defines.h>
#include <sincos.h>

/*************** Position berechnen  ++++++++++++++++++++*/


void Pos_odometrie(char flag_berechnung,unsigned short odo_links, unsigned short odo_rechts, short *pos_x,short *pos_y, short *richtung)  //Fahrtrichtung = "Norden" mit0 Grad
// Berechungen in Radiant*1000 für Fixkomma
{

    long  radius,odo_li_temp,odo_re_temp,richtung_odo;
    long pos_x_m, pos_y_m,pos_x_temp,pos_y_temp,pos_x_odo,pos_y_odo, weg;
    double r;
    char modus;
    long temp_weg,temp_weg_s,sin_x, cos_x,richtung_temp;
    long pos_xxx_temp, pos_yyy_temp;

    modus=0;
    
    pos_x_temp=(long)*pos_x; pos_y_temp=(long)*pos_y;
    richtung_odo=*richtung;

    // Weg in mm
    odo_li_temp=(long)(odo_links*Weg_tick)/100; odo_re_temp=(long)(odo_rechts*Weg_tick)/100; // 100 rausrechnen von Weg_Tick
    

    if ((abs(odo_links-odo_rechts)<120)||((odo_links>390)&&(odo_rechts>270))||((odo_rechts>390)&&(odo_links>270))) modus=1; // andere Berechnung da unendlicher Radius möglich
    else modus=0;

    
    weg= (odo_li_temp+odo_re_temp); // 10/2 kommt später, hier für Fixkomma um *20 zu groß
    Gesamter_Weg=Gesamter_Weg+weg/2; // Weg in mm

    if (flag_berechnung==1)  // Für Berechnung nach dem Zurückfahren Richtung um 3141 geändert
    {
        richtung_odo=richtung_odo-180;
    }


    if (modus==0) // Kurvenfahrt
    {
        richtung_temp=((odo_re_temp-odo_li_temp)*Radstand)/100;//Drehwinkel
        radius= abs((weg*50)/richtung_temp); //Drehrichtung // weg/2(mittelwert)/10(in cm)*1000(rad festkomma)
        
        
        
        sin_x =sinus(richtung_odo);
        cos_x =cosinus(richtung_odo);
//        r=(richtung_odo*35)/2000;
//        sin_x =(long)(128*sin(r));
//        cos_x =(long)(128*cos(r));
        

        if (odo_re_temp>=odo_li_temp) //linkskurve
        {
            pos_x_m=pos_x_temp-(radius*cos_x)/(long)128; // Drehpunkt im rechten Winkel zur Fahrtrichtung
            pos_y_m=pos_y_temp+(radius*sin_x)/(long)128; //Winkel Fahrtrichtung entsprechend Kompasswinkel!!
        }

        else                  //rechtskurve
        {
            pos_x_m=pos_x_temp+(radius*cos_x)/(long)128; // Drehpunkt
            pos_y_m=pos_y_temp-(radius*sin_x)/(long)128;
        }
        
        if (flag_berechnung==1)  // Für Berechnung beim Zurückfahren Richtung
        {richtung_temp=richtung_temp*(-1);}
        
        richtung_temp=(richtung_temp*2)/35; // Umrechung rad 1000 in Grad
        sin_x =sinus(richtung_temp);
        cos_x =cosinus(richtung_temp);

//r=richtung_temp/1000;
//        sin_x =(long)(128*sin(r));
//        cos_x =(long)(128*cos(r));


        // Neue Position nach Drehen um Drehpunkt, Drehwinkel im math Sinn!
        
        pos_xxx_temp = (long)pos_x_m +((((long)pos_x_temp-(long)pos_x_m)*cos_x)/(long)128) - ((((long)pos_y_temp-(long)pos_y_m)*sin_x)/(long)128); // 128 von sin/cos wieder rausrechnen
        pos_yyy_temp = (long)pos_y_m +((((long)pos_y_temp-(long)pos_y_m)*cos_x)/(long)128) + ((((long)pos_x_temp-(long)pos_x_m)*sin_x)/(long)128);

        
        
        pos_x_odo =(long)pos_xxx_temp;
        pos_y_odo =(long)pos_yyy_temp;

        richtung_odo= richtung_odo-(long)richtung_temp; // -richtung_temp als Korrektur da dieser als math Winkel berechnet wurde und für die Fahrtrichtung das Vorzeichen vertauscht werden muss

    }

    else    // gerade Fahrt
    {

        sin_x =sinus(richtung_odo);
        cos_x =cosinus(richtung_odo);
//        r=(richtung_odo*35)/2000;
//        sin_x =(long)(128*sin(r));
//        cos_x =(long)(128*cos(r));
        
        
        richtung_temp=((odo_li_temp-odo_re_temp)*Radstand)/100;//Drehwinkel
        
        if (flag_berechnung==1)  // Für Berechnung beim Zurückfahren Richtung
        {richtung_temp=richtung_temp*(-1);}
        
        // Weg in cm
        weg=(weg+5)/20; // +10 für besseres Runden
        temp_weg=(long)weg;
        
        temp_weg_s=((-temp_weg)*(long)richtung_temp+10)/(long)200; // letzten /10 werden unten berücksichtigt damit hier nicht zuviel gekürzt wird.

        pos_xxx_temp =(((temp_weg*sin_x)/(long)128) - ((temp_weg_s*cos_x)/(long)1280)); // 128 von sin/cos wieder rausrechnen
        pos_yyy_temp =(((temp_weg_s*sin_x)/(long)1280) + ((temp_weg*cos_x)/(long)128));
        
        pos_x_odo=pos_x_temp+(long)pos_xxx_temp;
        pos_y_odo=pos_y_temp+(long)pos_yyy_temp;

        richtung_odo= richtung_odo+(long)((richtung_temp*2)/35);
        
    }



    if (pos_x_odo <=0) {pos_x_odo=1;}
    if (pos_y_odo <=0) {pos_y_odo=1;}
    if (pos_x_odo>=pos_x_max) pos_x_odo=pos_x_max;
    if (pos_y_odo>=pos_y_max) pos_y_odo=pos_y_max;

    *pos_x=(short)pos_x_odo;
    *pos_y=(short)pos_y_odo;

    if (flag_berechnung==1)  // Für Berechnung nach dem Zurückfahren darf Kompass nicht verwendet werden da Richtung +3141 geändert
    {
        richtung_odo=richtung_odo+180;// Für Berechnung nach dem Zurückfahren Richtung um 180 geändert
    }
    *richtung=(short)richtung_odo;
}

#define Weg_tick ((long)(133)) //mm 1.33
#define Radstand ((long)(320)) //mm
#define Raddurchmesser ((long)(175))
#define TicksUmdrehung ((long)(414)) //mm

#define winkelfaktor (36000/((Radstand*2*TicksUmdrehung)/Raddurchmesser)+1)  ///24 

extern long Gesamter_Weg;

extern void Pos_odometrie(char flag_zurueck,unsigned short odo_links, unsigned short odo_rechts, short *pos_x,short *pos_y, short *richtung);

Mit den original Sinus/Cosinusfunktion funktioniert die Berechnung, 4x in der Sekunde, max 15cm Weg in 1/4s.
Mit der Eigenbau Funktion errechnet er gute >5m pro Berechnung.