Hallo,
jetzt habe ich mein eigenes Programm mit Befehlen für Linie und Kurve für das Haus des Nikolaus noch mal verbessert. Das Ergebnis habe ich mal wie versprochen abgelichtet.
Hier der Quellcode meines Programmes mit PID-Regler, bei Bedarf noch mit Abgleich der beiden Radgeschwindigkeiten gegeneinander (Faktor Kr) und zu guter letzt mit Anlauf- und Verzögerungsrampe:
Code:
#include "asuro.h"
/* ---------- Haus des Nikolaus ------------- */
/* Es soll das Haus des Nikolaus auf ein Blatt
Papier gezeichnet werden */
void linie(int weg, int geschw);
void kurve(int grad, int geschw);
int main(void)
{
Init();
int seite = 200; // Breite des Hauses ist gleich die Höhe bis UK-Dach in mm
int haus_dia = (unsigned int)((2 * 14142) / 100); // Diagonale des Hauses in mm
int vLinie = 200; // 200 mm/sec.
int vKurve = 100; // Kurve langsamer
char taste = 1;
while(taste == 1)
{
SerWrite("\n\r == Asuro faehrt Nikolaushaus. ==",35);
SerWrite("\n\r ================================",35);
SerWrite("\n\r Batterie: ",13); PrintInt(Batterie());
Msleep(1000);
linie ((seite), vLinie); // Strecke 1
Msleep(500);
kurve(-90, vKurve);
Msleep(500);
linie ((seite), vLinie); // Strecke 2
Msleep(500);
kurve(-135, vKurve);
Msleep(500);
linie (haus_dia, vLinie); // Strecke 3
Msleep(500);
kurve(135, vKurve);
Msleep(500);
linie (seite, vLinie); // Strecke 4
Msleep(500);
kurve(45, vKurve);
Msleep(500);
linie (((haus_dia/2)), vLinie); // Strecke 5
Msleep(500);
kurve(90, vKurve);
Msleep(500);
linie (haus_dia / 2, vLinie); // Strecke 6
Msleep(500);
kurve(-225, vKurve); // 135 korrekt
Msleep(500);
linie (seite, vLinie); // Strecke 7
Msleep(500);
kurve(-135, vKurve);
Msleep(500);
linie (haus_dia, vLinie); // Strecke 8
do
{
taste = PollSwitch(); taste = PollSwitch();taste = PollSwitch();
}
while (taste != 1);
}
while(1);
return 0;
}
/* END Haus des Nikolaus ------------------- */
und weiter
Code:
#include "asuro.h"
/* ---------- kurve ------------- */
/* empfängt die Daten für die Kurven-
Fahrt des Asuro
Diesmal soll der Bot quasi auf
der Stelle wenden (Drehpunkt ist
Mitte der Achse) */
void bewegen(int *f_weg, int w_geschw);
void kurve(int grad, int geschw)
{
int aus_weg[2];
long weg_hilf;
//weg_hilf = abs((1780L * (long)grad) / 1000L); //AS_SPURWEITE(102 mm) * 3.1415 * grad/180
weg_hilf = abs((1832L * (long)grad) / 1000L); //AS_SPURWEITE(105 mm) * 3.1415 * grad/180
if (grad < 0) // nach links drehen
{
aus_weg[0] = (int)(weg_hilf/2L*(-1L)); // linkes Rad zurück
aus_weg[1] = (int)(weg_hilf/2L); // rechtes Rad vorwärts
}
else if (grad > 0) // nach rechts drehen
{
aus_weg[0] = (int)(weg_hilf/2L); // linkes Rad vorwärts
aus_weg[1] = (int)(weg_hilf/2L*(-1L)); // rechtes Rad zurück
}
else // 0 Grad drehen
{
aus_weg[0] = 0; // linkes Rad steht
aus_weg[1] = 0; // rechtes Rad steht
}
bewegen(aus_weg, geschw);
}
/* END kurve ------------------- */
Code:
#include "asuro.h"
/* ---------- linie ------------- */
/* empfängt die Daten für die gerade
Fahrt des Asuro */
void bewegen(int *f_weg, int w_geschw);
void linie(int weg, int geschw)
{
int in_weg[2];
in_weg[0] = weg;
in_weg[1] = weg;
bewegen(in_weg, geschw);
}
/* END linie ------------------- */
Code:
#include "asuro.h"
//#define SIMULATION TRUE
struct MRDat
{
unsigned int tmpGefaWegAlt; // alter gefahrener Weg seit letztem Aufruf von HoleGefWeg()
int duty; // Motorspeed, Werte von min bis max
int e, e_alt, e_sum; // Regelabweichung der Geschwindigkeit
unsigned int x; // Istgeschwindigkeit
};
struct MRDat pid[2];
struct MRDatAll
{
int max, min; // wenn alles funktioniert auf unsigned char gehen
int Kp; // Proportionalitätsfaktor
char Ki; // Integralfaktor
int Kd; // Differentialfaktor
int Kr; // Proportionalitätsfaktor zur gegenseitigen Ausregelung der beiden Räder
int k_duty; // Konstanter Wert, der bei der Regelung zu duty addiert wird
unsigned int Ta; // Zeit für die Reglerschleife in ms, // Regelintervall in Millisekunden
int KdDivTa; // Kd / Ta
};
struct MRDatAll pidAlle;
void HoleGefWeg(unsigned int *WegData, char *Drehrichtung);
void SetMotDir(int *f_weg);
void SikMotDir(int *f_weg, char *RadRichtg);
void regeln_li(struct MRDat *pid, struct MRDatAll *Alles, unsigned int tmp_w); // Regelgrößen berechnen
void regeln_re(struct MRDat *pid, struct MRDatAll *Alles, unsigned int tmp_w); // Regelgrößen berechnen
void duty_limit(struct MRDat *pid, struct MRDatAll *pidAll, char *FlagRadfertig);
void set_speed(struct MRDat *pid);
void ausgabe(int *f_weg, unsigned int *GesGefaWeg, struct MRDat *pid, int w_geschw);
void bewegen(int *f_weg, int w_geschw)
{
char RadRichtg[2]; // sichere die Richtung der zu fahrende Wege
unsigned int tmpGefaWeg[2]; // gefahrener Weg seit letztem Aufruf von HoleGefWeg()
unsigned int tmp_w; // Sollwert der Geschwindigkeit darf sich bei den Rampen ändern
unsigned int GesGefaWeg[2]; // gesamte gefahrene Wegstrecken linkes, bzw. rechtes Rad
unsigned int ZuFahrendeStrecke[2]; // gesamte zu fahrende Wegstrecken linkes, bzw. rechtes Rad,
unsigned char AnfRampe = 90;// 100; // Länge der Anfahrrampe in mm
unsigned char BremsRampe = 90; //10; // Länge der Bremsrampe in mm
char RadFertig[2]; // wird TRUE, wenn das jeweilige Rad den vorgeschriebenen
// Weg zurückelegt hat
unsigned int long tmpZeit, tmpRegelZeit, tmpOdoZeit;
char Todo; // Zeit in ms, in der die Odoscheiben abgefragt werden
RadFertig[0] = RadFertig[1] = FALSE;
pid[0].e = pid[0].e_alt = pid[1].e = pid[1].e_alt = 0; // div. Initialisierungen
pid[0].e_sum = pid[1].e_sum = 0;
pid[0].duty = pid[1].duty = 0;
pid[0].x = pid[1].x = 0;
/***********************************************************/
pidAlle.k_duty = 120; //125;
pidAlle.Kp = 20;//20;//50; // darin steckt der Faktor 10 --> wird in der Formel korrigiert
pidAlle.Ki = 3;//30 // Werte von 0 bis 20 sinnvoll? Das ergibt eine Zuwachs von duty im Extremfall von
// Ki*Ta*e_sum = 20 * 10 * 100 = 20000!!! Zu hoch , deshalb /1000 ' 20 für Änderung für Motorspeed.
pidAlle.Kd = 3;//3;// 200; // Werte: z.B.: 100,200,300...
// darin steckt der Faktor 10 --> wird in der Formel korrigiert
// --> Vermeidung von float-Zahl;
pidAlle.Kr = 0; //;10;
pidAlle.Ta = 20; // Tickzeit in msec
Todo = 1; // alle xx ms die Odoscheiben abfragen
pidAlle.max = 255; // max-Wert für Motorspeed
pidAlle.min = 0; // min-Wert für Motorspeed
/***********************************************************/
pidAlle.KdDivTa = (pidAlle.Kd * 200) / pidAlle.Ta/10; // Die Zahl 200 multipl- und die 10 dividiert den "großen" Kd-Wert wieder raus
SetMotDir(f_weg); // Drehsinn der Motoren festlegen
SikMotDir(f_weg, RadRichtg); // Drehrichtung der Räder sichern
GesGefaWeg[0] = GesGefaWeg[1] = 0;
pid[0].tmpGefaWegAlt = pid[1].tmpGefaWegAlt = 0;
ZuFahrendeStrecke[0] = f_weg[0] * RadRichtg[0]; //Asuro soll ??? mm fahren, // Strecke wird angepasst: aus Minus muss Plus werden
ZuFahrendeStrecke[1] = f_weg[1] * RadRichtg[1]; //Asuro soll ??? mm fahren
tmpRegelZeit = tmpOdoZeit = Gettime();
HoleGefWeg(tmpGefaWeg, RadRichtg); // zur Initialisierung
// solange fahren, bis die Strecke zurückgelegt wurde
while ((GesGefaWeg[0] < ZuFahrendeStrecke[0]) || (GesGefaWeg[1] < ZuFahrendeStrecke[1]))
{
tmpZeit = Gettime();
if (tmpZeit >= (tmpOdoZeit + (unsigned int long)Todo)) //dann ist Tickzeit für Odoabfrage verstrichen
{
tmpOdoZeit = tmpZeit;
HoleGefWeg(tmpGefaWeg, RadRichtg); // zwischenzeitl. gef. Weg holen
//if (!RadFertig[0])
{ // Fehlzählungen vermeiden
GesGefaWeg[0] = GesGefaWeg[0] + tmpGefaWeg[0]; // zurückgelegte Strecke linkes Rad
pid[0].tmpGefaWegAlt = pid[0].tmpGefaWegAlt + tmpGefaWeg[0];
}
//if (!RadFertig[1])
{
GesGefaWeg[1] = GesGefaWeg[1] + tmpGefaWeg[1]; // zurückgelegte Strecke rechtes Rad
pid[1].tmpGefaWegAlt = pid[1].tmpGefaWegAlt + tmpGefaWeg[1];
}
}
/********************************************************************************/
// Rampe berücksichtigen
if (ZuFahrendeStrecke[0] >= (AnfRampe + BremsRampe)) // nur Rampe wenn nötig
{
if ((GesGefaWeg[0]+GesGefaWeg[1])/2 <= AnfRampe) // Anfahrrampe
tmp_w = w_geschw * (GesGefaWeg[0]+GesGefaWeg[1])/2/AnfRampe; // Sollgeschwindigkeit langsam ansteigen lassen
else if ((GesGefaWeg[0]+GesGefaWeg[1])/2 > (ZuFahrendeStrecke[0] - BremsRampe)) // Bremsrampe
tmp_w = w_geschw *(ZuFahrendeStrecke[0] - (GesGefaWeg[0]+GesGefaWeg[1])/2)/BremsRampe; // Sollgeschwindigkeit langsam fallen lassen
else tmp_w = w_geschw;
}
else // die beiden Rampen sind zusammen kürzer als die zufahrenden Strecke
{
if ((GesGefaWeg[0]+GesGefaWeg[1])/2 <= ZuFahrendeStrecke[0]/2)
// bis zur Mitte der Strecke beschleunigen
tmp_w = w_geschw * (GesGefaWeg[0]+GesGefaWeg[1])/2/AnfRampe; // Sollgeschwindigkeit langsam ansteigen lassen
else
// Sollgeschwindigkeit langsam fallen lassen, aber von dem Stand, der am Ende der Rampe errreicht wurde!
tmp_w = ((w_geschw*10)/BremsRampe)*(ZuFahrendeStrecke[0]-(GesGefaWeg[0]+GesGefaWeg[1])/2)/10;
}
/*******************************************************************************/
tmpZeit = Gettime();
if (tmpZeit >= (tmpRegelZeit + (unsigned int long)pidAlle.Ta)) //dann ist Tickzeit verstrichen
{
tmpRegelZeit = tmpZeit; //Zeit merken
// würde mal sagen, er sollte jetzt regeln...
if (GesGefaWeg[0] < ZuFahrendeStrecke[0]) // Gesamtstrecke für linkes Rad gefahren?
regeln_li(&pid[0], &pidAlle, tmp_w); // Regelgrößen berechnen
else
{
RadFertig[0] = TRUE;
pid[0].duty = 0;
//BackLED(ON, OFF); // BackLED-Routine kollidiert mit Odometriemessung
}
if (GesGefaWeg[1] < ZuFahrendeStrecke[1]) // Gesamtstrecke für rechtes Rad gefahren?
regeln_re(&pid[0], &pidAlle, tmp_w); // &pid[0] ist richtig!! da hier die Adresse des 1. Elementes übergeben wird
else
{
RadFertig[1] = TRUE;
pid[1].duty = 0;
//BackLED(OFF, ON); // BackLED-Routine kollidiert mit Odometriemessung
}
if (GesGefaWeg[0] != GesGefaWeg[1]) //Räder gegeneinander regeln
{
pid[1].duty = (pid[1].duty + (int)((GesGefaWeg[0]-GesGefaWeg[1]) * pidAlle.Kr));
}
duty_limit(&pid[0], &pidAlle, RadFertig); // limitiert die duty-Werte
set_speed(&pid[0]);
}
}
MotorSpeed(0,0); // alle Maschinen stop!
MotorDir(BREAK,BREAK);
//BackLED(OFF, OFF); // BackLED-Routine kollidiert mit Odometriemessung
ausgabe(f_weg, GesGefaWeg, &pid[0], w_geschw); // Protokoll ausgeben
}
void SetMotDir(int *f_weg)
{
if ((f_weg[0] >= 0) && (f_weg[1] >= 0))
MotorDir(FWD,FWD);
else if ((f_weg[0] < 0) && (f_weg[1] > 0))
{
MotorDir(RWD,FWD);
//f_weg[0] = (f_weg[0] * -1);
}
else if ((f_weg[0] > 0) && (f_weg[1] < 0))
{
MotorDir(FWD,RWD);
//f_weg[1] = (f_weg[1] * -1);
}
else
{
MotorDir(RWD,RWD);
//f_weg[0] = (f_weg[0] * -1);
//f_weg[1] = (f_weg[1] * -1);
}
}
void SikMotDir(int *f_weg, char *RadRichtg)
{
if(f_weg[0] > 0) RadRichtg[0] = 1; //sichere die Drehrichtung der Räder
else RadRichtg[0] = -1;
if(f_weg[1] > 0) RadRichtg[1] = 1; //sichere die Drehrichtung der Räder
else RadRichtg[1] = -1;
}
void regeln_li(struct MRDat *pid, struct MRDatAll *Alles, unsigned int tmp_w) // Regelgrößen berechnen
{
// ***** PID-Regelung linkes Rad *******
pid[0].x = pid[0].tmpGefaWegAlt * 1000 / pidAlle.Ta ; // Istgeschwindigkeit x = s/t
pid[0].e = tmp_w - pid[0].x; // Vergleich, z.B. Asuro ist zu schnell, e wird negativ
pid[0].e_sum = pid[0].e_sum + pid[0].e; // I-Anteil aufaddieren
if (pid[0].e_sum > 350) pid[0].e_sum = 350;
if (pid[0].e_sum < -350) pid[0].e_sum = -350;
pid[0].duty = (pidAlle.k_duty + pidAlle.Kp * pid[0].e / 10) + (int)((int)pidAlle.Ki * pidAlle.Ta * pid[0].e_sum / 1000) + pidAlle.KdDivTa * (pid[0].e - pid[0].e_alt)/5; // PID-Reglergleichung
pid[0].tmpGefaWegAlt = 0; // alten Wert speichern
pid[0].e_alt = pid[0].e; // wieder zurücksetzen
}
void regeln_re(struct MRDat *pid, struct MRDatAll *Alles, unsigned int tmp_w) // Regelgrößen berechnen
{
// ***** PID-Regelung rechtes Rad *******
pid[1].x = pid[1].tmpGefaWegAlt * 1000/ pidAlle.Ta;
pid[1].e = tmp_w - pid[1].x;
pid[1].e_sum = pid[1].e_sum + pid[1].e; // I-Anteil aufaddieren
if (pid[1].e_sum > 350) pid[1].e_sum = 350;
if (pid[1].e_sum < -350) pid[1].e_sum = -350;
pid[1].duty = (pidAlle.k_duty + pidAlle.Kp * pid[1].e / 10) + (int)((int)pidAlle.Ki * pidAlle.Ta * pid[1].e_sum / 1000) + pidAlle.KdDivTa * (pid[1].e - pid[1].e_alt)/5;
pid[1].tmpGefaWegAlt = 0;
pid[1].e_alt = pid[1].e;
}
void duty_limit(struct MRDat *pid, struct MRDatAll *pidAll, char *FlagRadfertig)
{
if(!FlagRadfertig[0])
{
if (pid[0].duty > pidAlle.max) pid[0].duty = pidAlle.max; // pos. Begrenzung
if (pid[0].duty < pidAlle.min) pid[0].duty = pidAlle.min; // neg. Begrenzung
}
if(!FlagRadfertig[1])
{
if (pid[1].duty > pidAlle.max) pid[1].duty = pidAlle.max; // pos. Begrenzung
if (pid[1].duty < pidAlle.min) pid[1].duty = pidAlle.min; // neg. Begrenzung
}
}
void ausgabe(int *f_weg, unsigned int *GesGefaWeg, struct MRDat *pid, int w_geschw)
{
SerWrite("\n\r ==== Strecke gefahren! ====",30);
SerWrite("\n\r Sollstrecke links.: ",23); PrintInt(f_weg[0]);
SerWrite("\n\r Ist gesamt links.: ",23); PrintInt(GesGefaWeg[0]);
SerWrite("\n\r Ist gesamt rechts: ",23); PrintInt(GesGefaWeg[1]);
SerWrite("\n\r Sollgeschwindigk..: ",23); PrintInt(w_geschw);
SerWrite("\n\r pid[0].duty...........: ",23); PrintInt(pid[0].duty);
SerWrite("\n\r pid[1].duty...........: ",23); PrintInt(pid[1].duty);
}
void set_speed(struct MRDat *pid)
{
MotorSpeed(pid[0].duty, pid[1].duty);// Motor anschalten
}
Code:
#define MM_PRO_SEGWE 1; // zurückgelegter Weg für 1 Odo-Segmentwechsel in Millimeter
#include "asuro.h"
void HoleGefWeg(unsigned int *WegData, char *Drehrichtung)
{
unsigned int data[2];
unsigned int tmpWertOdoLi, tmpWertOdoRe; // Helligkeitswerte der Odoscheiben links / rechts
unsigned int static tmpWertOdoLiAlt, tmpWertOdoReAlt; // Helligkeitswerte der Odoscheiben links / rechts
// behalten!! ---> static Definition
unsigned int WegLinks, WegRechts; // zurückgelegter Weg des linken bzw. rechten Rades
unsigned int OdoWertReHell; // Odo-Wert für linke Odoscheibe, ab dem eine weiße Fläche angenommen wird
unsigned int OdoWertReDkl; // Odo-Wert für linke Odoscheibe, ab dem eine dunkle Fläche angenommen wird
unsigned int OdoWertLiHell; // Odo-Wert für rechte Odoscheibe, ab dem eine weiße Fläche angenommen wird
unsigned int OdoWertLiDkl; // Odo-Wert für rechte Odoscheibe, ab dem eine dunkle Fläche angenommen wird
unsigned int dNeutrZoneLi; // Neutrale Zone der Odowerte
unsigned int dNeutrZoneRe; // Neutrale Zone der Odowerte
switch(Drehrichtung[1]) //Drehrichtung rechtes Rad
{
case 1: //positiv, d.h. vorwärts
{
OdoWertReHell = 700; // Odo-Wert für rechte Odoscheibe, ab dem eine weiße Fläche angenommen wird
OdoWertReDkl = 700; // Odo-Wert für rechte Odoscheibe, ab dem eine dunkle Fläche angenommen wird
break;
}
case -1: // positiv, d.h. rückwärts
{
OdoWertReHell = 650; // bei Rückwärtsfahrt ändern sich rechts die Werte
OdoWertReDkl = 650;
break;
}
default:
{
OdoWertReHell = 700; // Odo-Wert für rechte Odoscheibe, ab dem eine weiße Fläche angenommen wird
OdoWertReDkl = 700; // Odo-Wert für rechte Odoscheibe, ab dem eine dunkle Fläche angenommen wird
}
}
OdoWertLiHell = 500; // Odo-Wert für linke Odoscheibe, ab dem eine weiße Fläche angenommen wird
OdoWertLiDkl = 500; // Odo-Wert für linke Odoscheibe, ab dem eine dunkle Fläche angenommen wird
dNeutrZoneLi = 5; // Neutrale Zone der Odowerte
dNeutrZoneRe = 5; // Neutrale Zone der Odowerte
OdoWertReHell = OdoWertReHell - dNeutrZoneRe;
OdoWertReDkl = OdoWertReDkl + dNeutrZoneRe;
OdoWertLiHell = OdoWertLiHell - dNeutrZoneLi;
OdoWertLiDkl = OdoWertLiDkl + dNeutrZoneLi;
WegLinks = 0;
WegRechts = 0; // Wegzähler zurücksetzen
OdometrieData(data); // Odo-Daten lesen
OdometrieData(data); // Odo-Daten lesen
tmpWertOdoLi = data[0];
tmpWertOdoRe = data[1]; // Werte zwischenspeichern
// hoher Odo-Messwert = "schwarzer" oder dunkler Helligkeitswert
// kleiner Odo-Messwert = "weißer" oder großer Helligkeitswert
// Weg der linken Scheibe messen
if ((tmpWertOdoLiAlt < OdoWertLiHell) && (tmpWertOdoLi > OdoWertLiDkl))
{
WegLinks = WegLinks + MM_PRO_SEGWE // Segmentwechsel w/sw hat stattgefunden, Weg addieren
tmpWertOdoLiAlt = tmpWertOdoLi;
}
else if ((tmpWertOdoLiAlt > OdoWertLiDkl) && (tmpWertOdoLi < OdoWertLiHell))
{
WegLinks = WegLinks + MM_PRO_SEGWE // Segmentwechsel sw/w hat stattgefunden, Weg addieren
tmpWertOdoLiAlt = tmpWertOdoLi;
}
else
{
// noch keine Tätigkeiten
}
// Weg der rechten Scheibe messen
if ((tmpWertOdoReAlt < OdoWertReHell) && (tmpWertOdoRe > OdoWertReDkl))
{
WegRechts = WegRechts + MM_PRO_SEGWE // Segmentwechsel w/sw hat stattgefunden, Weg addieren
tmpWertOdoReAlt = tmpWertOdoRe;
}
else if ((tmpWertOdoReAlt > OdoWertReDkl) && (tmpWertOdoRe < OdoWertReHell))
{
WegRechts = WegRechts + MM_PRO_SEGWE // Segmentwechsel sw/w hat stattgefunden, Weg addieren
tmpWertOdoReAlt = tmpWertOdoRe;
}
else
{
// noch keine Tätigkeiten
}
WegData[0] = WegLinks; // Werte ins Array zwecks Übergabe ans aufrufende Proggi in Array schreiben
WegData[1] = WegRechts;
}
Das Hex-File habe ich auch angehängt.
Grüße, Hans.
Zitieren
Zitat von Hans Z
Berechtigungen
Lesezeichen