| Die letzten 3 Beiträge |
| eulerscheZahl |
Doch, aber
- das ist ziemlich viel Code, den liest man sich nicht mal eben in 10 Minuten durch und versteht ihn dann
- zur Ausführen des Codes fehlt RP6RobotBaseLib.h. Wenn man sich die herunterlädt, fehlen danach einige andere Headerdateien, die man sich auch alle zusammensuchen müsste.
Dazu hat wohl niemand Lust. |
| ThomasThomas |
Na niemand ? 
|
| ThomasThomas |
Welchen Weg durchläuft der Roboter?
Meine Frage:
Hallo Leute,
könnt Ihr mir vielleicht helfen zu sagen, wie der Roboter bzw. das Auto in ca. fahren wird, wenn das Programm wie folgt aussieht:
Ich hoffe Ihr könnt mir helfen.
Gruß,
Thomas
#include "RP6RobotBaseLib.h"
#include <math.h>
#define MAXPUNKTE 120
#define zeitschritt 100
#define feldschritt 2.0 // mindestens alle 2 cm Feld checken
#define wegschritt 5.0 // alle 5 cm Weggroessen merken
#define winkelschritt 0.53 // oder alle 30 Grad
#define fakr 0.0229808
#define fakl 0.0229779
#define abstand 20.8049
#define vkette 80
#define fwinkel 2350.0
#define fstrecke 11080.0
#define ein_meter 1.0
#define v_innen 0
#define v_aussen 80
#define NIX_LOS 0
#define TREFFER_MITTE 1
#define TREFFER_LINKS 2
#define TREFFER_RECHTS 3
#define ES_GEHT_WEITER 1
#define MACH_WENDUNG 2
#define SETZ_ZURUECK 3
#define ES_GEHT_RUECKWAERTS 4
#define FAHR_VORWAERTS 5
#define ES_GEHT_VORWAERTS 6
#define GO_ON 7
#define FAHR_WEG_VON_WAND 8
#define ES_GEHT_WEG_VON_WAND 9
#define CHECK_ABSTAND 10
#define MACH_CHECK_ABSTAND 11
#define CHECK_STRECKE 12
#define MACH_CHECK_STRECKE 13
#define TEST 14
#define TEST_RUECKWAERTS 15
#define TEST_RUECKWAERTS_AUSFUEHRUNG 16
#define TEST_AUSGLEICH 17
#define TEST_AUSGLEICH_AUSFUEHRUNG 18
#define TEST_VORWAERTS 19
#define TEST_VORWAERTS_AUSFUEHRUNG 20
#define SMAX 4
#define ZMAX 4
#define KANTE_X 34.5
#define KANTE_Y 34.5
#define MAXFELDER 30
#define LHALBE 10.5
#define BHALBE 6.5
typedef struct
{
uint16_t el, er, elvor, ervor;
float v, vvor, s, svor, phi, phivor;
float x, xvor, y, yvor;
float phiabs,phiabsvor; // |phi|
uint8_t merkzeiger; // zeigt auf Wertepaar X,Y
float sziel, phiziel; // Weg bzw. Winkel, bei dem spaetestens
// wieder abgespeichert wird
uint16_t msek; // Dauer einer begrenzten Bewegung
uint8_t zustand;
uint16_t zeit_fuer_phase0,zeit_fuer_phase1,zeit_fuer_phase2;
uint8_t St_zustand; // Stossstangenzustand
uint8_t sp_vor, zl_vor; // zuletzt besuchtes Feld
uint8_t feldzeiger; // zeigt auf Wertepaar Sp,Zl
float fziel; // Weg, bei dem spaetestens
// das Feld ueberprueft wird
uint8_t sp,zl; // aktuelles Feld
float horizontal,vertikal; // Ausdehnung bei Schiefstand phi
uint8_t zuerst_rechts; // dann links, wenn true
uint8_t Linksdrehung; // gleichbedeutend mit zuerst_rechts
uint8_t s0,z0; // Start Strahl
uint8_t s1,z1; // Ende Strahl
uint8_t anfang; // Hilfsgroesse
float steigung; // des Strahls
uint8_t smin,zmin; // neues Ziel (Ende des Strahls mit minimalen Besuchszahlen)
float minwert; // bezogene Besuchszahlen
float xmin, ymin, xmax, ymax; // in ausdehnung berechnen
float deltaphi; // Winkel in Rad
float alpha_1;
float strecke;
} fahrt;
fahrt F;
typedef int16_t i16feld[MAXPUNKTE];
i16feld X,Y; // Ort in [mm]
typedef int i8feld[MAXFELDER];
i8feld Sp,Zl;
uint8_t M[SMAX][ZMAX];
float zweipi, pi, pihalbe, rad2deg, deg2rad;
float rad40, rad45, rad50, rad90, rad130, rad135, rad140, rad180;
uint8_t sup(uint8_t a, uint8_t b) { if (a>b) return a; return b; }
uint8_t inf(uint8_t a, uint8_t b) { if (a<b) return a; return b; }
void init_fahrtenbuch(uint8_t snull, uint8_t znull)
{
uint8_t s,z;
for (s=0; s<SMAX; s++)
for (z=0; z<ZMAX; z++) M[s][z] = 0;
M[snull][znull] = 1;
F.sp_vor = snull;
F.zl_vor = znull;
Sp[0] = snull;
Zl[0] = znull;
F.feldzeiger = 1;
F.fziel = feldschritt;
X[0] = Y[0] = 0;
F.merkzeiger = 1;
F.sziel = wegschritt;
F.phiziel = winkelschritt;
}
void update_fahrt(void)
{
F.elvor = F.el; F.ervor = F.er;
F.vvor = F.v; F.svor = F.s;
F.phivor = F.phi; F.xvor = F.x; F.yvor = F.y;
F.phiabsvor = F.phiabs;
}
void schreibf(char *text, float x)
{
char zahl[20];
dtostrf(x,6,4,zahl);
writeString(text);
writeString(zahl);
writeChar(' ');
}
void schreibfln(char *text, float x)
{
char zahl[20];
dtostrf(x,6,4,zahl);
writeString(text);
writeString(zahl);
writeChar('\n');
}
void speicher_fahrt(void)
{
uint8_t ausgeben = false;
if ((F.s>F.sziel)&&(F.merkzeiger<MAXPUNKTE))
{
F.sziel += wegschritt;
ausgeben = true;
}
if ((F.phiabs>F.phiziel)&&(F.merkzeiger<MAXPUNKTE))
{
F.phiziel += winkelschritt;
ausgeben = true;
}
if (ausgeben)
{
X[F.merkzeiger] = (int16_t) (F.x*10.0); // in [mm]
Y[F.merkzeiger] = (int16_t) (F.y*10.0); // in [mm]
F.merkzeiger++;
schreibf("x = ",F.x);
schreibf("y = ",F.y);
schreibfln("phi = ",rad2deg*F.phi);
}
}
void schreibi8(char *text, int x)
{
writeString(text);
writeInteger(x,DEC);
writeChar(' ');
}
void schreibi8ln(char *text, int x)
{
writeString(text);
writeInteger(x,DEC);
writeChar('\n');
}
void ausdehnung(void)
{
//writeString_P("ausdehnung\n");
float sphi = fabs(sin(F.phi)); float cphi = fabs(cos(F.phi));
F.horizontal = LHALBE*cphi + BHALBE*sphi;
F.vertikal = LHALBE*sphi + BHALBE*cphi;
//schreibf("F.horizontal ",F.horizontal);
//schreibf("F.vertikal ",F.vertikal);
F.xmin = F.horizontal; F.xmax = SMAX*KANTE_X - F.horizontal;
F.ymin = F.vertikal; F.ymax = ZMAX*KANTE_Y - F.vertikal;
}
uint8_t welche_spalte(void)
{
//writeString_P("welche_spalte\n");
float rand = F.horizontal - F.x;
if (rand>0.0) F.x += rand;
rand = F.x + F.horizontal - SMAX*KANTE_X;
if (rand>0.0) F.x -= rand;
//schreibf("x = ",F.x);
int s = (int) (F.x/KANTE_X);
if (s<0) s = 0;
else if (s>SMAX-1) s = SMAX - 1;
//schreibi8ln("-> sp = ",s);
return s;
}
uint8_t welche_zeile(void)
{
//writeString_P("welche_zeile\n");
float rand = F.vertikal - F.y;
if (rand>0.0) F.y += rand;
rand = F.y + F.vertikal - ZMAX*KANTE_Y;
if (rand>0.0) F.y -= rand;
//schreibf("y = ",F.y);
int zl = (int) (F.y/KANTE_Y);
if (zl<0) zl = 0;
else if (zl>ZMAX-1) zl = ZMAX - 1;
//schreibi8ln("-> zl = ",zl);
return zl;
}
void check_feld(void)
{
//writeString_P("check_feld\n");
ausdehnung();
F.sp = welche_spalte();
F.zl = welche_zeile();
if ((F.sp!=F.sp_vor)||(F.zl!=F.zl_vor))
{
M[F.sp][F.zl]++;
F.sp_vor = F.sp;
F.zl_vor = F.zl;
//schreibi8ln("sp = ",F.sp);
//schreibi8ln("zl = ",F.zl);
if (F.feldzeiger<MAXFELDER)
{
Sp[F.feldzeiger] = F.sp;
Zl[F.feldzeiger] = F.zl;
F.feldzeiger++;
}
}
}
void speicher_feld(void)
{
if (F.s>F.fziel)
{
check_feld();
F.fziel += feldschritt;
}
else if (F.St_zustand!=NIX_LOS)
check_feld();
}
void wo_bin_ich(uint16_t mSek)
{
update_fahrt();
F.el = getLeftDistance();
F.er = getRightDistance();
uint8_t richtung = getDirection();
float dsr = fakr*(float) (F.er - F.ervor);
if ((richtung==BWD)||(richtung==RIGHT)) dsr = -dsr;
float dsl = fakl*(float) (F.el - F.elvor);
if ((richtung==BWD)||(richtung==LEFT)) dsl = -dsl;
float deltaT = (float) mSek*0.001;
F.v = (dsr + dsl)/deltaT - F.vvor;
F.s = 0.5*fabs(dsr + dsl) + F.svor;
float dphi = (dsr - dsl)/abstand;
F.phiabs = F.phiabsvor + fabs(dphi);
F.phi = F.phivor + dphi;
F.x = F.xvor + 0.5*(F.v*cos(F.phi) + F.vvor*cos(F.phivor))*deltaT;
F.y = F.yvor + 0.5*(F.v*sin(F.phi) + F.vvor*sin(F.phivor))*deltaT;
speicher_fahrt();
speicher_feld();
}
void init_fahrt(void)
{
F.el = 0; F.er = 0;
F.v = 0.0; F.s = 0.0; F.phi = 0.0;
F.x = 0.0; F.y = 0.0;
F.phiabs = 0.0;
F.msek = 0;
F.St_zustand = NIX_LOS; // Voreinstellung StossStange
}
void anhalten(void)
{
writeString_P("anhalten\n");
stop();
setStopwatch1(0);
while (getStopwatch1()<300) task_RP6System();
}
void fahre(void)
{
task_RP6System(); // beinhaltet task_motionControl();
if (getStopwatch2()>zeitschritt)
{
wo_bin_ich(getStopwatch2());
setStopwatch2(0);
}
}
void schreibd(float x)
{
char zahl[10];
dtostrf(x,5,1,zahl);
writeString(zahl);
}
void schreibXY(int16_t x, int16_t y) // gibt [mm]-Koordinaten in [cm] aus
{
float w;
w = ((float) x)*0.1; schreibd(w);
writeChar(' ');
w = ((float) y)*0.1; schreibd(w);
writeChar('\n');
}
void hin_rauf_runter(int16_t x)
{
int16_t y = (int16_t) (ZMAX*KANTE_Y*10); // in mm
schreibXY(x,0);
schreibXY(x,y); schreibXY(x,0);
}
void hin_rechts_links(int16_t y)
{
int16_t x = (int16_t) (SMAX*KANTE_X*10); // in mm
schreibXY(0,y);
schreibXY(x,y); schreibXY(0,y);
}
void schachbrett(void)
{
writeString_P("Schachbrett\n");
uint8_t zl,sp;
int16_t x = 0;
hin_rauf_runter(x);
for (sp=1; sp<=SMAX; sp++)
{
x = (int16_t) (sp*KANTE_X*10); // in mm
hin_rauf_runter(x);
}
int16_t y = 0;
hin_rechts_links(y);
for (zl=1; zl<=ZMAX; zl++)
{
y = (int16_t) (zl*KANTE_Y*10); // in mm
hin_rechts_links(y);
}
}
void schreibFeld(int8_t sp, int8_t zl)
{
// gibt die Mittelpunktskoordinaten des Feldes (zl,sp) in [cm] aus
float w;
w = ((float) sp + 0.5)*KANTE_X; schreibd(w);
writeChar(' ');
w = ((float) zl + 0.5)*KANTE_Y; schreibd(w);
writeChar('\n');
}
void fahrtenbuch(void)
{
writeString_P("Fahrtenbuch\n");
uint8_t i;
for (i=0; i<F.merkzeiger; i++) schreibXY(X[i],Y[i]);
writeString_P("Feldprotokoll\n");
for (i=0; i<F.feldzeiger; i++) schreibFeld(Sp[i],Zl[i]);
schachbrett();
}
void begrenzte_bewegung(uint8_t r, uint16_t msek, uint8_t vl, uint8_t vr)
{
writeInteger(F.zustand,DEC);
writeString_P(" begrenzte_bewegung\n");
F.msek = msek;
changeDirection(r);
moveAtSpeed(vl,vr);
setStopwatch1(0);
setStopwatch2(0);
}
void starteStoppuhren(void)
{
writeString_P("starteStoppuhren\n");
startStopwatch1();
startStopwatch2();
}
uint8_t genug_gefahren(void)
{
if (getStopwatch1()<F.msek)
{
fahre();
return false;
}
else
{
anhalten();
wo_bin_ich(getStopwatch2());
return true; //fertig
}
}
void unbegrenzte_bewegung(uint8_t r, uint8_t v)
{
writeInteger(F.zustand,DEC);
writeString_P(" unbegrenzte_bewegung\n");
changeDirection(r);
moveAtSpeed(v,v);
setStopwatch2(0);
}
void sieh_nach_stossstange(void)
{
if (bumper_left && bumper_right)
F.St_zustand = TREFFER_MITTE;
else if (bumper_left)
F.St_zustand = TREFFER_LINKS;
else if (bumper_right)
F.St_zustand = TREFFER_RECHTS;
}
void was_sagt_stossstange(void)
{
switch(F.St_zustand)
{
case TREFFER_MITTE:
case TREFFER_LINKS:
case TREFFER_RECHTS:
if (F.zustand==ES_GEHT_WEITER)
F.zustand = MACH_WENDUNG;
break;
default:;
}
F.St_zustand = NIX_LOS;
}
int licht_brennt(void)
{
uint16_t hl = readADC(ADC_LS_L), hr = readADC(ADC_LS_R);
return (hl>800) || (hr>800);
}
int kein_Licht(void)
{
uint16_t hl = readADC(ADC_LS_L), hr = readADC(ADC_LS_R);
return (hl<200) && (hr<200);
}
void warte_auf_licht(void)
{
writeString_P("warte_auf_licht\n");
uint8_t fertig=false;
uint8_t lichter=0b000111;
setLEDs(lichter);
setStopwatch1(0);
while(!fertig)
{
fertig = fertig || licht_brennt();
if (getStopwatch1()>200)
{
lichter = ~lichter;
setLEDs(lichter);
setStopwatch1(0);
}
}
setLEDs(0);
}
void warte_auf_dunkel(void)
{
writeString_P("warte_auf_dunkel\n");
uint8_t fertig=false;
uint8_t lichter=0;
setLEDs(lichter);
setStopwatch1(0);
while(!fertig)
{
fertig = fertig || kein_Licht();
if (getStopwatch1()>600)
{
lichter = ~lichter;
setLEDs(lichter);
setStopwatch1(0);
}
}
setLEDs(0);
}
float korrektes_intervall(float ph)
{
while (ph>pi) ph -= zweipi;
while (ph<=-pi) ph += zweipi;
return ph;
}
void M_ausgleich(void)
{
uint8_t s,z,max,min;
max = min = M[0][0];
for (s=0; s<SMAX; s++)
for (z=0; z<ZMAX; z++) { max = sup(max,M[s][z]); min = inf(min,M[s][z]); }
if (min>0)
{
for (s=0; s<SMAX; s++)
for (z=0; z<ZMAX; z++) M[s][z] -= min;
max -= min; min = 0;
}
}
void check_min(float wert)
{
if (F.anfang)
{
F.smin = F.s1;
F.zmin = F.z1;
F.minwert = wert;
F.anfang = false;
}
else if (wert<F.minwert)
{
F.smin = F.s1;
F.zmin = F.z1;
F.minwert = wert;
}
}
void entlang_z(int off)
{
int8_t z,s;
uint8_t n = 0;
float sum = 0.0;
for (z = F.z0 + off; z != F.z1+off; z+=off)
{
s = F.s0 + (int8_t) (F.steigung*(float)(z-F.z0));
sum += (float) M[s][z];
writeInteger(s,DEC); writeInteger(z,DEC); writeChar(' ');
n++;
}
schreibf("zsum=",sum);
schreibi8("zn= ",n);
schreibfln("min = ",sum/n);
check_min(sum/n);
}
void entlang_s(int off)
{
int8_t z,s;
uint8_t n = 0;
float sum = 0.0;
for (s = F.s0+off; s != F.s1+off; s+=off)
{
z = F.z0 + (int8_t)(F.steigung*(float)(s-F.s0));
sum += (float) M[s][z];
writeInteger(s,DEC); writeInteger(z,DEC); writeChar(' ');
n++;
}
schreibf("ssum=",sum);
schreibi8("sn= ",n);
schreibfln("min = ",sum/n);
check_min(sum/n);
}
void strahl(int s1, int z1)
{
writeInteger(s1,DEC); writeChar(' ');
writeInteger(z1,DEC); writeChar(' ');
F.s1 = s1;
F.z1 = z1;
float ds = (float) F.s1 - (float) F.s0,
dz = (float) F.z1 - (float) F.z0;
if (fabs(ds)>fabs(dz))
{
F.steigung = dz/ds;
if (ds>0.0) entlang_s(1);
else entlang_s(-1);
}
else
{
F.steigung = ds/dz;
if (dz>0.0) entlang_z(1);
else entlang_z(-1);
}
}
void fallADF(void)
{
writeString_P("Fall ADF\n");
int s,z = ZMAX - 1;
if (F.z0<ZMAX-1) for (s=0; s<SMAX-1; s++) strahl(s,z);
s = SMAX - 1;
for (z=ZMAX-1; z>=0; z--) strahl(s,z);
if (F.z0>0) for (s=SMAX-2; s>=0; s--) strahl(s,0);
}
void fallCEH(void)
{
writeString_P("Fall CEH\n");
int s,z = ZMAX-1;
if (F.z0<ZMAX-1) for (s=SMAX-1;s>0;s--) strahl(s,z);
for (z=ZMAX-1; z>=0; z--) strahl(0,z);
if (F.z0>0) for (s=1; s<SMAX; s++) strahl(s,0);
}
void fallG(void)
{
writeString_P("Fall G\n");
int s,z;
for (z=0; z<ZMAX; z++) strahl(0,z);
z = ZMAX-1;
for (s=1; s<SMAX; s++) strahl(s,z);
s = SMAX-1;
for (z=ZMAX-2 ; z>=0; z--) strahl(s,z);
}
void fallB(void)
{
writeString_P("Fall B\n");
int s,z;
for (z=ZMAX-1; z>=0; z--) strahl(0,z);
for (s=1; s<SMAX; s++) strahl(s,0);
s = SMAX-1;
for (z=1; z<ZMAX; z++) strahl(s,z);
}
float richtung(void)
{
M_ausgleich();
F.s0 = F.sp; F.z0 = F.zl;
F.anfang = true;
if (F.s0==0) fallADF();
else if (F.s0==SMAX-1) fallCEH();
else if (F.z0==0) fallG();
else fallB();
return atan2(F.zmin-F.z0,F.smin-F.s0);
}
void schreib_drehung(float deltaphihalbe)
{
if (F.Linksdrehung) schreibfln("Linksdrehung um ",rad2deg*deltaphihalbe);
else schreibfln("Rechtsdrehung um ",rad2deg*deltaphihalbe);
}
uint8_t AoderE(void){ return (F.sp==0) && (F.zl==ZMAX-1); }
uint8_t BoderF(void){ return (F.sp==SMAX-1) && (F.zl==ZMAX-1); }
uint8_t CoderG(void){ return (F.sp==0) && (F.zl==0); }
uint8_t DoderH(void){ return (F.sp==SMAX-1) && (F.zl==0); }
uint8_t kleinerOderGroesser(float u, float o)
{
return (F.phi<u) || (F.phi>o);
}
uint8_t imIntervall(float,float); // gibt es schon weiter unten
uint8_t pruefe_ecken(void)
{
uint8_t p = false, lichter;
if (AoderE())
{
lichter = 0b001000; setLEDs(lichter);
p = (kleinerOderGroesser(-rad135,rad135) && F.Linksdrehung)
|| (imIntervall(rad45,rad135) && !F.Linksdrehung);
}
else if (BoderF())
{
lichter = 0b010000; setLEDs(lichter);
p = (imIntervall(-rad45,rad45) && !F.Linksdrehung)
|| (imIntervall(rad45,rad135) && F.Linksdrehung);
}
else if (CoderG())
{
lichter = 0b000001; setLEDs(lichter);
p = (kleinerOderGroesser(-rad135,rad135) && !F.Linksdrehung)
|| (imIntervall(-rad135,-rad45) && F.Linksdrehung);
}
else if (DoderH())
{
lichter = 0b000010; setLEDs(lichter);
p = (imIntervall(-rad45,rad45) && F.Linksdrehung)
|| (imIntervall(-rad135,-rad45) && !F.Linksdrehung);
}
return p;
}
uint8_t planung(void)
{
writeString_P("planung\n");
F.phi = korrektes_intervall(F.phi);
float phisoll = richtung();
float delta_phi = phisoll - F.phi;
delta_phi = korrektes_intervall(delta_phi);
F.Linksdrehung = F.zuerst_rechts = delta_phi>0.0;
float deltaphihalbe = 0.5*fabs(delta_phi);
schreib_drehung(deltaphihalbe);
if (pruefe_ecken())
{
writeString_P("Ausweichmanoever noetig: ");
F.zeit_fuer_phase0
= F.zeit_fuer_phase1 = (uint16_t) (fwinkel*rad40);
F.zeit_fuer_phase2 = (uint16_t) (fwinkel*2.0*deltaphihalbe);
writeString_P("zeit_fuer_phase0/1 = ");
writeInteger(F.zeit_fuer_phase0,DEC);
writeString_P(", zeit_fuer_phase2 = ");
writeInteger(F.zeit_fuer_phase2,DEC);
writeChar('\n');
return FAHR_WEG_VON_WAND;
}
else
{
F.zeit_fuer_phase0 = 0;
F.zeit_fuer_phase1
= F.zeit_fuer_phase2
= (uint16_t) (fwinkel*deltaphihalbe);
writeString_P("zeit_fuer_phase1/2 = ");
writeInteger(F.zeit_fuer_phase1,DEC);
writeChar('\n');
return SETZ_ZURUECK;
}
}
void init_P(void)
{
deg2rad = atan(1.0)/45.0;
rad2deg = 1.0/deg2rad;
pi = 4.0*atan(1.0);
zweipi = pi + pi;
pihalbe = 0.5*pi;
rad40 = deg2rad*40.0;
rad45 = deg2rad*45.0;
rad50 = deg2rad*50.0;
rad90 = pihalbe;
rad130 = deg2rad*130.0;
rad135 = deg2rad*135.0;
rad140 = deg2rad*140.0;
rad180 = pi;
}
void set_phi2(float unten, float oben)
{
if (F.phi<unten) F.phi = unten;
else if (F.phi>oben) F.phi = oben;
ausdehnung();
}
void set_phi(float wert)
{
set_phi2(wert,wert);
}
void set_phi3(float wu,float u,float w,float o,float wo)
{
if (F.phi<u) set_phi(wu);
else if (F.phi>o) set_phi(wo);
else set_phi(w);
}
void spUnten(void) { F.sp = 0; F.x = F.xmin; }
void spOben(void) { F.sp = SMAX-1; F.x = F.xmax; }
void zlUnten(void) { F.zl = 0; F.y = F.ymin; }
void zlOben(void) { F.zl = ZMAX-1; F.y = F.ymax; }
uint8_t imIntervall(float u, float o) { return (F.phi>u) && (F.phi<=o); }
void raus(char* text) { writeString(text); writeChar(' '); }
void nplA(void) { raus("nplA"); set_phi2(-rad135,-rad90); zlUnten(); }
void nplB(void) { raus("nplB"); set_phi2(-rad45, 0.0); spOben(); }
void nplC(void) { raus("nplC"); set_phi2( rad45, rad90); zlOben(); }
void nplD(void) { raus("nplD"); set_phi2(rad135, rad180); spUnten(); }
void links_nicht_plausibel(void)
{
raus("lnp");
if (imIntervall(-rad180,-rad90)) nplA();
else if (imIntervall( -rad90, 0.0)) nplB();
else if (imIntervall( 0.0, rad90)) nplC();
else nplD();
}
void nprE(void) { raus("nprE"); set_phi2(-rad180,-rad135); spUnten(); }
void nprF(void) { raus("nprF"); set_phi2( -rad90, -rad45); zlUnten(); }
void nprG(void) { raus("nprG"); set_phi2( 0.0, rad45); spOben(); }
void nprH(void) { raus("nprH"); set_phi2( rad90, rad135); zlOben(); }
void rechts_nicht_plausibel(void)
{
raus("rnp");
if (imIntervall(-rad180,-rad90)) nprE();
else if (imIntervall( -rad90, 0.0)) nprF();
else if (imIntervall( 0.0, rad90)) nprG();
else nprH();
}
void npmA(void) { raus("npmA"); set_phi(-rad135); spUnten(); zlUnten(); }
void npmB(void) { raus("npmB"); set_phi( -rad90); zlUnten(); }
void npmC(void) { raus("npmC"); set_phi( -rad45); spOben(); zlUnten(); }
void npmD(void) { raus("npmD"); set_phi( 0.0); spOben(); }
void npmE(void) { raus("npmE"); set_phi( rad45); spOben(); zlOben(); }
void npmF(void) { raus("npmF"); set_phi( rad90); zlOben(); }
void npmG(void) { raus("npmG"); set_phi( rad135); spUnten(); zlOben(); }
void npmHJ(void) { raus("npmHJ"); set_phi(rad180); spUnten(); }
void mitte_nicht_plausibel(void)
{
raus("mnp");
if (imIntervall(-rad140,-rad130)) npmA();
else if (imIntervall(-rad130, -rad50)) npmB();
else if (imIntervall( -rad50, -rad40)) npmC();
else if (imIntervall( -rad40, rad40)) npmD();
else if (imIntervall( rad40, rad50)) npmE();
else if (imIntervall( rad50, rad130)) npmF();
else if (imIntervall( rad130, rad140)) npmG();
else npmHJ();
}
void TL1(void) { raus("TL1"); set_phi2( -rad45, 0.0); spOben(); }
void TL2(void) { raus("TL2"); set_phi2(-rad135,-rad90); zlUnten(); }
void TL3(void) { raus("TL3"); set_phi2( rad135, rad180); spUnten(); }
void TL4(void) { raus("TL4"); set_phi2( rad45, rad90); zlOben(); }
void TR5(void) { raus("TR5"); set_phi2( 0.0, rad45); spOben(); }
void TR6(void) { raus("TR6"); set_phi2( -rad90, -rad45); zlUnten(); }
void TR7(void) { raus("TR7"); set_phi2(-rad180,-rad135); spUnten(); }
void TR8(void) { raus("TR8"); set_phi2( rad90, rad135); zlOben(); }
void TM9(void)
{ raus("TM9"); set_phi3(0.0,rad40,rad45,rad50,rad90);
spOben(); zlOben(); }
void TM11(void)
{ raus("TM11"); set_phi3(-rad90,-rad50,-rad45,-rad40,0.0);
spOben(); zlUnten(); }
void TM13(void)
{ raus("TM13"); set_phi3(-rad180,-rad140,-rad135,-rad130,-rad90);
spUnten(); zlOben(); }
void TM15(void)
{ raus("TM15"); set_phi3(rad90,rad130,rad135,rad140,rad180);
spUnten(); zlUnten();}
void TM10(void) { raus("TM10"); set_phi(0.0); spOben(); }
void TM12(void) { raus("TM12"); set_phi(-rad90); zlUnten(); }
void TM14(void) { raus("TM14"); set_phi(rad180); spUnten(); }
void TM16(void) { raus("TM16"); set_phi(rad90); zlOben(); }
void plausibel(uint8_t stossstange)
{
if (stossstange==TREFFER_LINKS)
{
writeString_P("TREFFER_LINKS\n");
if (F.sp==SMAX-1) TL1();
else if (F.zl==0) TL2();
else if (F.sp==0) TL3();
else if (F.zl==ZMAX-1) TL4();
else links_nicht_plausibel();
}
else if (stossstange==TREFFER_RECHTS)
{
writeString_P("TREFFER_RECHTS\n");
if (F.sp==SMAX-1) TR5();
else if (F.zl==0) TR6();
else if (F.sp==0) TR7();
else if (F.zl==ZMAX-1) TR8();
else rechts_nicht_plausibel();
}
else if (stossstange==TREFFER_MITTE)
{
writeString_P("TREFFER_MITTE\n");
if ((F.sp==SMAX-1)&&(F.zl==ZMAX-1)) TM9();
else if ((F.sp==SMAX-1)&&(F.zl==0)) TM11();
else if ((F.sp==0)&&(F.zl==0)) TM13();
else if ((F.sp==0)&&(F.zl==ZMAX-1)) TM15();
else if (F.sp==SMAX-1) TM10();
else if (F.zl==0) TM12();
else if (F.sp==0) TM14();
else if (F.zl==ZMAX-1) TM16();
else mitte_nicht_plausibel();
}
raus("\n");
schreibi8("plausibel: sp = ",F.sp);
schreibi8("zl = ",F.zl);
schreibf("x = ",F.x);
schreibf("y = ",F.y);
schreibfln("phi = ",rad2deg*F.phi);
}
uint8_t bumper_kontrolle(void)
{
if ((bumper_left)&&(bumper_right)) return TREFFER_MITTE;
if (bumper_left) return TREFFER_LINKS;
if (bumper_right) return TREFFER_RECHTS;
return NIX_LOS;
}
void lichterkreisel(void)
{
uint8_t nr = 1;
while (true)
{
switch (nr)
{
case 1: setLEDs(0b000001); nr = 2; break;
case 2: setLEDs(0b000010); nr = 3; break;
case 3: setLEDs(0b000100); nr = 6; break;
case 6: setLEDs(0b100000); nr = 5; break;
case 5: setLEDs(0b010000); nr = 4; break;
default: setLEDs(0b001000); nr = 1;
}
mSleep(300);
}
}
int main(void)
{
init_P();
initRobotBase();
BUMPERS_setStateChangedHandler(sieh_nach_stossstange);
starteStoppuhren();
powerON();
init_fahrt();
init_fahrtenbuch(0,0);
F.zustand = TEST; //GO_ON; // CHECK_STRECKE; // CHECK_ABSTAND; // GO_ON;
uint8_t fertig = false;
while (!fertig)
{
was_sagt_stossstange();
switch (F.zustand)
{
case MACH_WENDUNG:
anhalten();
plausibel(bumper_kontrolle());
F.zustand = TEST();
break;
case FAHR_WEG_VON_WAND:
if (F.zuerst_rechts) //heisst, hier muss Linkskurve gefahren werden
begrenzte_bewegung(BWD,F.zeit_fuer_phase0,v_innen,v_aussen);
else
begrenzte_bewegung(BWD,F.zeit_fuer_phase0,v_aussen,v_innen);
F.zustand = ES_GEHT_WEG_VON_WAND;
break;
case ES_GEHT_WEG_VON_WAND:
if (genug_gefahren()) F.zustand = SETZ_ZURUECK;
break;
case SETZ_ZURUECK:
if (F.zuerst_rechts)
begrenzte_bewegung(BWD,F.zeit_fuer_phase1,v_aussen,v_innen);
else
begrenzte_bewegung(BWD,F.zeit_fuer_phase1,v_innen,v_aussen);
F.zustand = ES_GEHT_RUECKWAERTS;
break;
case ES_GEHT_RUECKWAERTS:
if (genug_gefahren())
{
anhalten();
F.zustand = FAHR_VORWAERTS;
}
break;
case FAHR_VORWAERTS:
if (F.zuerst_rechts)
begrenzte_bewegung(FWD,F.zeit_fuer_phase2,v_innen,v_aussen);
else
begrenzte_bewegung(FWD,F.zeit_fuer_phase2,v_aussen,v_innen);
F.zustand = ES_GEHT_VORWAERTS;
break;
case ES_GEHT_VORWAERTS:
if (genug_gefahren())
{
anhalten();
F.zustand = GO_ON;
}
break;
case GO_ON:
unbegrenzte_bewegung(FWD,vkette);
F.zustand = ES_GEHT_WEITER;
break;
case ES_GEHT_WEITER: fahre();
break;
case CHECK_STRECKE:
begrenzte_bewegung(FWD,fstrecke*ein_meter,vkette,vkette);
F.zustand = MACH_CHECK_STRECKE;
break;
case MACH_CHECK_STRECKE:
if (genug_gefahren())
{
fertig = true;
schreibfln("s = 100 cm? ",F.s);
}
break;
case CHECK_ABSTAND:
begrenzte_bewegung(FWD,fwinkel*pi,v_aussen,v_innen);
F.zustand = MACH_CHECK_ABSTAND;
break;
case MACH_CHECK_ABSTAND:
if (genug_gefahren())
{
fertig = true;
schreibfln("phi = 180? ",rad2deg*F.phi);
}
break;
case TEST:
writeString("zustand = TEST");
writeChar('\n');
F.deltaphi = -rad135; // Winkel in Rad
if (F.deltaphi > 0){
F.alpha_1 = F.deltaphi - rad90;
}else{
F.alpha_1 = F.deltaphi + rad90;
}
schreibfln("alpha_1 = ", F.alpha_1);
F.strecke = 0.1 * (1 - sin(fabs(F.alpha_1)));
F.zeit_fuer_phase0 = (uint16_t) (fwinkel*fabs(F.alpha_1)); // Zeit für kurve rückwärts
F.zeit_fuer_phase1 = (uint16_t) (fstrecke*F.strecke); // Zeit für Rückwärtsfahren
F.zeit_fuer_phase2 = (uint16_t) (fwinkel*rad90); // Zeit für 90° kurve vorwärts
schreibfln("Zeit Phase 0 = ", F.zeit_fuer_phase0);
schreibfln("Zeit Phase 1 = ", F.zeit_fuer_phase1);
schreibfln("Zeit Phase 2 = ", F.zeit_fuer_phase2);
F.zustand = TEST_RUECKWAERTS;
break;
case TEST_RUECKWAERTS:
writeString("zustand = TEST_RUECKWAERTS");
writeChar('\n');
if (F.alpha_1 > 0){
begrenzte_bewegung(BWD,F.zeit_fuer_phase0,v_aussen,v_innen); // Rückwärts Rechtskurve
}else{
begrenzte_bewegung(BWD,F.zeit_fuer_phase0,v_innen,v_aussen); // Rückwärts Linkskurve
}
F.zustand = TEST_RUECKWAERTS_AUSFUEHRUNG;
break;
case TEST_RUECKWAERTS_AUSFUEHRUNG:
if (genug_gefahren())
{
anhalten();
F.zustand = TEST_AUSGLEICH;
}
break;
case TEST_AUSGLEICH:
writeString("zustand = TEST_AUSGLEICH");
writeChar('\n');
begrenzte_bewegung(BWD,F.zeit_fuer_phase1,vkette,vkette); // Rückwärts
F.zustand = TEST_AUSGLEICH_AUSFUEHRUNG;
break;
case TEST_AUSGLEICH_AUSFUEHRUNG:
if (genug_gefahren())
{
anhalten();
F.zustand = TEST_VORWAERTS;
}
break;
case TEST_VORWAERTS:
writeString("zustand = TEST_VORWAERTS");
writeChar('\n');
if (F.deltaphi > 0){
begrenzte_bewegung(FWD,F.zeit_fuer_phase2,v_innen,v_aussen); // Vorwärts Linkskurve
}else{
begrenzte_bewegung(FWD,F.zeit_fuer_phase2,v_aussen,v_innen); // Vorwärts Rechtskurve
}
F.zustand = TEST_VORWAERTS_AUSFUEHRUNG;
break;
case TEST_VORWAERTS_AUSFUEHRUNG:
if (genug_gefahren())
{
anhalten();
fertig = true;
}
break;
Meine Ideen:
Der Roboter fährt in einem viereckigen (2m x 2m) Kasten, gerade aus auf eine Wand.
Könnt ihr mir vielleicht skizzieren oder aufzählen, welche Schritte der Roboter dann durchfährt?
Vielen Dank ! |
|
|
