Problem Sinus durch eigene Funktion ersetzen

[damfino]

Ich will diese Funktion

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


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

durch diese ersetzen:


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);


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

Hallo,
hier würde ich ansetzen:


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

Müsste das nicht so aussehen:


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.


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 ;)


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


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

verwenden.

Ich hoffe, das hilft dir weiter:rolleyes:
Grüße, Bernhard

[damfino]

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]

zu früh gefreut, rechnet wieder nur Mist :(

[HaWe]

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

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]

@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]

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]

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.

[oberallgeier]

... 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:
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 (http://forum.diegeodaeten.de/index.php?mode=thread&id=2946#p2983).

[BMS]

@damfino: Ich meinte eigentlich ob du nochmal den neuen Teil der Sinusfunktion zeigen kannst, so wie du es umgesetzt hast.
Grüße, Bernhard

[HaWe]

wie Odometrie funktioniert, ist mir auch klar - und ich meinte auch keine allgemeinen Tabllen wie von Oberallgeier:

ich meinte (auch) DEINEN Tabellen-array-Sinus128 in Gegenüberstellung zum echten sin(x)*128, selber von dir aufgerufen so wie du es dann auch in deiner Anwendung machen willst.

[damfino]

So wie vorher, hatte nur die while Schleifen eingebaut.


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

//static const char sinusdaten[91] PROGMEM =
static const char 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,128};

short sinus(short winkel)
{
short result=0;
while(winkel<0){winkel+=360;}
while(winkel>=360){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;
while(winkel<0){winkel+=360;}
while(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);
}

Da die odomentrie Ergebnisse um den Faktor 100 abweichen müssen hier Werte weit größer als 128 rausgehen, aber wie?

[sternst]

static const char sinusdaten[92] PROGMEM =
^^^^

... pgm_read_word(&sinusdaten[winkel]));
^^^^

[damfino]

](*,)](*,)](*,)](*,)](*,)](*,)](*,)](*,)

Danke!!