Problem Sinus durch eigene Funktion ersetzen

**damfino** · 10.09.2015, 11:35

Ich will diese Funktion

Code:

r=(double)(richtung_temp); //in Radiant*1000
        r=r/1000; //in Radiant

        
        sin_x =(long)(128*sin(r));

durch diese ersetzen:

Code:

richtung_temp=(richtung_temp*2)/35; // von Radiant*1000 in Grad, zB 3141*2/35= 179, 1° Fehler ist egal

        sin_x =sinus(richtung_temp);

Code:

#include <sincos.h>
#include <avr/pgmspace.h> 

static const short sinusdaten[92] PROGMEM = 
{0,2,4,7,9,11,13,16,18,20,22,24,27,29,31
,33,35,37,40,42,44,46,48,50,52,54,56,58
,60,62,64,66,68,70,72,73,75,77,79,81,82
,84,86,87,89,91,92,94,95,97,98,99,101
,102,104,105,106,107,109,110,111,112,113
,114,115,116,117,118,119,119,120,121,122
,122,123,124,124,125,125,126,126,126,127
,127,127,128,128,128,128,128,128};

short sinus(short winkel)
{
short result=0;
if (winkel>360) winkel=winkel-360;
if (winkel<360) winkel=winkel+360;

if (winkel<=90) result = (short)(pgm_read_word(&sinusdaten[winkel]));
if ((winkel>90)&&(winkel<=180)) result = (short)(pgm_read_word(&sinusdaten[180-winkel]));
if ((winkel>180)&&(winkel<=270)) result = -(short)(pgm_read_word(&sinusdaten[winkel-180]));
if (winkel>270) result = -(short)(pgm_read_word(&sinusdaten[360-winkel]));

return (result);

short cosinus(short winkel)
{
short result=0;
if (winkel>360) winkel=360;

if (winkel<=90) result = (short)(pgm_read_word(&sinusdaten[90-winkel]));
if ((winkel>90)&&(winkel<=180)) result = -(short)(pgm_read_word(&sinusdaten[winkel-90]));
if ((winkel>180)&&(winkel<=270)) result = -(short)(pgm_read_word(&sinusdaten[270-winkel]));
if (winkel>270) result = (short)(pgm_read_word(&sinusdaten[winkel-270]));

return (result);
}

sin_x soll also immer den Sinuswert*128 entsprechen.

1. Grund ist Umstellung meiner Fahrfunktionen auf Grad ( also drehe 20° nach links) da ich mich mit Radiant noch nie anfreunden konnte.
2. Grund: die Eigenbau Berechnung läuft viel schneller als Fließkomma, die Genauigkeit ist ausreichend.

Problem: funktioniert nicht, die nachfolgende Positionsberechnung errechnet mehrere Meter Fahrweg anstatt nur cm.
Wenn man wieder die normale Sinus/Cosinus Berechnung nimmt rechnet es richtig, also liegt es nur an dieser Funktion

Ist die Tabelle falsch angelegt?

**BMS** · 10.09.2015, 13:19

Hallo,
hier würde ich ansetzen:

Code:

if (winkel<360) winkel=winkel+360;

Müsste das nicht so aussehen:

Code:

if (winkel<0) winkel=winkel+360;

Noch besser wäre eine while-Schleife, damit es z.B. auch für Winkel -500° oder +999° klappt? Mit den ersten beiden Abfragen wird ja nur 1x subtrahiert/addiert um den Winkel in den Bereich 0...360° zu bekommen.

Code:

while(winkel<0){winkel+=360;}
while(winkel>=360){winkel-=360;}

Die 4 IF-Abfragen kann man noch optimieren.
In deinem Fall werden ja immer alle 4 Abfragen geprüft.
Werden die letzten drei IF-Abfragen mit ELSEIF gemacht, dann werden im Durchschnitt nur noch 2,5 IF-Abfragen geprüft.
Mit einer sog. binären Suche werden immer nur 2 IF-Abfragen geprüft.
Das bringt noch einen klitzekleinen Geschwindigkeitsvorteil

Code:

if(winkel<=180)
{
  if(winkel<=90){/* 0...90° */}
  else{/* 90...180° */}
}
else
{
  if(winkel<=270){/* 180...270° */}
  else{/* 270...360° */}
}

Gibt deine Funktion dann die richtigen Werte für sinus(0), sinus(90), sinus(180), sinus(270), sinus(360), ... aus?

Die Cosinus-Funktion musst du auch entsprechend anpassen, oder einfach

Code:

short cosinus(short winkel)
{
  return sinus(winkel+90);
}

verwenden.

Ich hoffe, das hilft dir weiter

Grüße, Bernhard

**damfino** · 10.09.2015, 13:32

Danke, das hilft mir einiges weiter!

Kann gut sein dass die Eingangskorrektur zu klein war, die while Schleifen sind sicher besser.

Die Cosinus-Funktion musst du auch entsprechend anpassen, oder einfach
Code:
short cosinus(short winkel)
{
return sinus(winkel+90);
}
verwenden.

Das der Funktionen nur um 90° versetzt sind weiß ich ja, aber diese einfache Lösung ist mir überhaupt nicht eingefallen. Hatte wohl ein paar Bretter vorm Kopf

LG Werner

**damfino** · 11.09.2015, 17:38

zu früh gefreut, rechnet wieder nur Mist

HaWe · 11.09.2015, 17:50

ich habe da sicher was nicht richtig mitgekriegt... aber was genau macht die Funktion/der array

Code:

static const short sinusdaten[92] PROGMEM = 
{0,2,4,7,9,11,13,16,18,20,22,24,27,29,31
,33,35,37,40,42,44,46,48,50,52,54,56,58
,60,62,64,66,68,70,72,73,75,77,79,81,82
,84,86,87,89,91,92,94,95,97,98,99,101
,102,104,105,106,107,109,110,111,112,113
,114,115,116,117,118,119,119,120,121,122
,122,123,124,124,125,125,126,126,126,127
,127,127,128,128,128,128,128,128};

?

einen Sinuswert erhält man doch doch dadurch sicher nicht, denn Sinuswerte liegen ja immer zwischen -1 bis +1.

Aber was macht das dann?

edit - ah, jetzt hab ichs:

"sin_x soll also immer den Sinuswert*128 entsprechen.".

ok.

dann schreib doch mal eine Funktion, die die echten Sinuswerte für 0...360 deinen Tabellenwerten gegenüberstellt bzw. die Fehlerdifferenz ausgibt!

**BMS** · 11.09.2015, 17:58

@HaWe Eigentlich hat er es ja schon begründet:

... soll also immer den Sinuswert*128 entsprechen.

Das Rechnen mit Vielfachen ist sinnvoll, weil die Berechnung mit Ganzzahlen deutlich schneller abläuft als die Fließkomma-Berechnung.
Die Genauigkeit ist bei den meisten Anwendungen ausreichend.

@damfino: Zeige nochmal deinen aktuellen Code, wie genau hast du den Code getestet? Keine Panik, das bekommen wir schon hin

Grüße, Bernhard

HaWe · 11.09.2015, 18:01

ja, ich sah's gerade auch... hat sich überschnitten..
aber:

dann schreib doch mal eine Funktion, die die echten Sinuswerte für 0...360 deinen Tabellenwerten gegenüberstellt bzw. die Fehlerdifferenz ausgibt!

**damfino** · 11.09.2015, 18:39

Es wird damit der Weg der Odometrie berechnet:

Code:

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

Code:

#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.

**oberallgeier** · 11.09.2015, 18:52

... Das Rechnen mit Vielfachen ist sinnvoll, weil die Berechnung mit Ganzzahlen deutlich schneller abläuft als die Fließkomma-Berechnung ...

Ja - Ganzzahlen - prima bei kleinen Computern. Und mit ner guten Rechenformel braucht man auch keine Tabelle. Wir hatten mal das Problem für den ATAN auf nem mega8 gelöst, deutlich schneller als mit der GCC-Bibliothek und (für unsere Anwendung - Rückwärtsschnitt) ausreichende Genauigkeit (was um alles in der Welt sind schon <<1 % ????).

Auch wenns kein Sinus ist - nur als Beispiel:

Code:

    Näherungsweise Berechnung des atan                            N_atan     = Näherungsweiser ARCTAN nach der Formel aus                
                                    für 0 < x[rad] < π/2                
                                Beispiel :     = WENN( E9 < 1; E9 / ( 1 + 0,28 *E9 * E9 ); 1,5707963268 - E9 / ( E9 * E9  + 0,28 ) )                

    x [ Grad ]        x [ RAD ]    tan-Excel (x)        atan_Excel        N_atan        N_atan / atan         => Fehler %     
                                                    
     1        0,017453    0,017455065        0,017453        0,017454        1,00001624         0,002    
     5        0,087266    0,087488664        0,087266        0,087302        1,000402179        0,040    
    10        0,174533    0,176326981        0,174533        0,174805        1,001560063        0,156    
    20        0,349066    0,363970234        0,349066        0,350952        1,005404629        0,540    
    30        0,523599    0,577350269        0,523599        0,528064        1,008528467        0,853    
    40        0,698132    0,839099631        0,698132        0,700917        1,003990322        0,399    
    50        0,872665    1,191753593        0,872665        0,869879        0,996807742       -0,319    
    60        1,047198    1,732050808        1,047198        1,042732        0,995735766       -0,426    
    70        1,221730    2,74747742         1,221730        1,219844        0,99845582        -0,154    
    80        1,396263    5,671281822        1,396263        1,395991        0,999804992       -0,020    
    89,9      1,569051    572,9572411        1,569051        1,569051        1                  0,000    
                                                    
                                        Rückrechnung : Grad aus Arcustangens            
                                        Grad (Excel-atan)        Grad (N_atan)    
     1        0,017453    0,017455065        0,017453        0,017454         1         1,000    
     5        0,087266    0,087488664        0,087266        0,087302         5         5,002    
    10        0,174533    0,176326981        0,174533        0,174805        10        10,016    
    20        0,349066    0,363970234        0,349066        0,350952        20        20,108    
    30        0,523599    0,577350269        0,523599        0,528064        30        30,256    
    40        0,698132    0,839099631        0,698132        0,700917        40        40,160    
    50        0,872665    1,191753593        0,872665        0,869879        50        49,840    
    60        1,047198    1,732050808        1,047198        1,042732        60        59,744    
    70        1,221730    2,74747742         1,221730        1,219844        70        69,892    
    80        1,396263    5,671281822        1,396263        1,395991        80        79,984    
    89,9      1,569051    572,9572411        1,569051        1,569051        89,9      89,900

... und das ging schon recht flott - siehe Posting(s) im Geodaetenforum.