Welchen Weg durchläuft der Roboter? |
19.02.2015, 15:33 | Auf diesen Beitrag antworten » |
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 ! |
|
|
20.02.2015, 21:56 | Auf diesen Beitrag antworten » |
ThomasThomas | Na niemand ? |
21.02.2015, 09:12 | Auf diesen Beitrag antworten » |
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. |
|