Sensorik am RP6, Befehle und Allgemeines

[fabqu]

Hallo!
Ich mus nächste Woche ein kleines Projekt mit dem RP6 an meiner Uni machen und hätte da noch ein paar Fragen:

Ich habe die Beispielprogramme TV-Remote und Slave zusammengelegt, sodass ich den Bot also per IR-Fernbedienung steuern kann.
Jetzt möchte ich folgendes machen:
Der Bot bekommt einen IR-Befehl "Dort, wo du jetzt bist, das ist nun dein Startpunkt!!!". Dann fahre ich den Bot einfach in der Gegend herum, wies mir eben so passt. Irgendwann sage ich dem Bot mit einem neuen Code: "So, das ist nun dein Zielpunkt".
Während der Ganzen Fahrt misst der Bot Strecke und Drehwinkel mit den Odometern und "weiß" daher stets, wo in seinem Koordinatensystem mit x- und y-Koordinaten und "Start" als Koordinatenursprung, er sich befindet. Also weiß er auch, wo er am Zielpunkt ist und in welchem Winkel er gerade steht.
Am Ende kommt der Befehl "Fahre direkt zum Start zurück!" und er fährt direkt auf den Startfleck zu, weicht Hindernissen aus und nimmt nach jedem Hinderniss wieder den direktesten Weg ein.
Dass das nicht supergenau wird, weiß ich natürlich schon...

Nun zu meinen Fragen:
1.: Das wichtigste: Ist der Bot dazu fähig? Hat er dafür genügend Speicher? Kann er sinus, cosinus und tangens?
2.: Soll die Base oder die M32 rechnen? Mir wäre Base lieber...
3.: Wie kann ich die Odometer stets abfragen? Immer, wenn ein Fernbedienungsknopf losgelassen wurde, muss der Bot Strecke und Winkel in ein Register schreiben (wohl in ein Array) und x und y berechnen sowie seinen Winkel.
4.: Das Projekt startet Donnerstag und soll schon Freitag wieder fertig sein, also heut in einer Woche. Es wäre der Hammer, wenn ich Mittwoch bis Freitag hier jemanden erreichen könnte falls Akute Fragen bestehen!!!!


So, danke Euch schon mal :p
Fabian

[RolfD]

Hallo,
da sich die Fehler (Winkel, Fehler duch Reibung/durchrutschen) in dieser Anordnung aufaddieren, ist das nicht einfach zu lösen - müsste aber gehen. Die Base reicht dafür sicherlich und wenn du nicht Kilometer zurück legst langt auch der Speicher mit ca. 1,5 Kb. Da auf M32 und Base beide CPUs gleich sind, ist der Vorteil einer M32 mit vielen freien Ports kaum gegeben. Für die Odometrie bietet die RP6lib Funktionen, welche auf der RP6 CD ist. Die wirst Du aber ggf. abändern müssen. Trigonometrische Funktionen lassen sich mit der libc oder mit eigenen Mathelibs umsetzen. Sowas findet sich mit Google für AVRs. Ich halte das für machbar, habe aber meine Zweifel an der Genauigkeit. "Ungefähr" wird der Bot aber zurück finden. Von einem Master/Slave Konzept würde ich dringend abraten da das I2C nicht sonderlich stabil ist. Für Zuhause ist das egal aber als Projekt an der Uni kommt es nicht gut wenn der Bot streikt oder unwillig reagiert.
LG Rolf

[Fabian E.]

Prinzipiell bin ich mir nicht ganz sicher, ob du das so hinbekommst...
Wie "genau" muss das ganze denn sein?
Vor allem bei Drehungen stimmen die Werte eben gut und gerne schon mal auf +-20° nicht...
Du könntest dir Räder an den RP6 bauen, damit sollte es genauer gehen.

Vom Speicher her musst du dir halt was schönes ausdenken, wie du das speichern willst.
Du könntest hier aber auch die M32 nehmen und in den EEPROM schreiben. Das ist ganz easy.

CPU auf Base und M32 sind in der Tat die selben, allerdings ist die M32 doppelt so schnell getaktet (16Mhz). Ob das ein Vorteil ist glaube ich kaum...
Du willst ja nicht sonst was berechnen...

@Rolf, ich denke schon, dass er I2C benutzen kann. Bevor du deine Stresstests gemacht hast,
hat sich ja noch nie jemand beschwert, die Probleme tauchen also nur unter bestimmten Bedingungen auf.

Also wie gesagt, bastel doch mal schnell ein Programm mit einem festem Ablauf drinne (2m vor, 30° drehen, 3m vor, 70° drehen, 1m zurück) und danach das ganze eben noch mal von hinten.
Dann "sollte" er ja wieder am Anfang stehen... Wird er aber nicht ;)

[fabqu]

Nun ja, da ich das ganze in einem Raum mit rutschfestem, gleichmäßigem Boden mache, denke ich, dass sich der Schlupf der Ketten gering hält. Klar, in einem Wohnzimmer mit teils Hochflor-Teppich, teils Parkett und noch ein bisschen Fließen dazu, gibts da so seine Probleme...
Ich hatte mir das so gedacht: Array anlegen: long int ArrayName[30][30] für 30 "Operationen", erste Spalte für die Strecken, zweite für die Winkel. Der Einfachheit halber werde ich zunächst nur geradeaus fahren und mich auf der Stelle Drehen. Kurven lass ich mal weg...
Eigentlich liegt mein Hauptproblem noch in der Odometerabfrage. Es gibt ja Befehle für "Fahre soundso weit", "drehe soundso viele Grad". Was ich brauche ist eine Abfrage der Strecke, die bei Tastendruck der taste z.B. "Vorwärts" beginnt und die gefahrene Strecke ausgiebt, sobald der Bot stehen bleibt.
Da finde ich keinen schon fertig gebastelten Befehl?!
D.h., den müsste ich selber basteln, oder? Also auf die Lichtschranke zugreifen und dann schwarze/weiße Streifen zählen und nach Manual die Strecke berechnen, oder?

Danke Euch schon mal!

[Fabian E.]

Wie gesagt, probier es einfach mal aus.
Die aktuelle Strecke kannst du definitiv auslesen, beim Winkel weiß ich es gerade nicht.
Ansonsten lässt den RP6 immer fest vorgegeben 45° drehen oder so und machst das dann paar mal hintereinander.

[fabqu]

Zu dem Auslesen der Strecke hab ich was in der Lib.c gefunden:

/************************************************** ***************************/
// Encoders

// Timing variable used for speed calculation:
volatile uint8_t speed_timer;

// Speed measurement variables
volatile uint16_t mleft_counter;
volatile uint16_t mright_counter;
volatile uint16_t mleft_speed;
volatile uint16_t mright_speed;

// Distance
volatile uint16_t mleft_dist;
volatile uint16_t mright_dist;

// This is only used for the selftest program.
// You don't need this for your own programs!
#ifdef DEBUG_MEASURE_DUTY_CYCLE
volatile uint16_t cycle_h_l;
volatile uint16_t cycle_l_l;
volatile uint16_t cycle_h_r;
volatile uint16_t cycle_l_r;

volatile uint8_t cycle_h_l_tmp;
volatile uint8_t cycle_l_l_tmp;
volatile uint8_t cycle_h_r_tmp;
volatile uint8_t cycle_l_r_tmp;
#endif

/**
* External Interrupt 0 ISR
* (ENCL)
*
*/
ISR (INT0_vect)
{
mleft_dist++;
mleft_counter++;

// Only used for selftest program:
#ifdef DEBUG_MEASURE_DUTY_CYCLE
if(isEncoderLeft()) {
cycle_l_l = cycle_l_l_tmp;
cycle_l_l_tmp = 0;
}
else {
cycle_h_l = cycle_h_l_tmp;
cycle_h_l_tmp = 0;
}
#endif
}

Ich denke, das müsste es sein, oder? Dann kann ich einfach zu Beginn eines Fahr-Vorgangs mleft_dist und mright_dist auf Null setzen und danach auslesen, ins Array schreiben und Koordinaten berechnen?!?!?! :confused:

MfG

[Fabian E.]

Korrekt. Anhand der unterschiedlichen Strecken könnte man sich übrigens auch mit etwas Mathe den Winkel ausrechen ;)

[fabqu]

Meinst du jetzt, dass ich mir nach einem Drehmanöver den gedrehten Winkel berechne? Dafür gibts glaube ich auch schon vorgefertigte Funktionen für rotate etc.
Oder meinst du, dass ich mir nur die gesamt gefahrenen Strecken der linken und rechten Kette ansehe, durch die Differenz aus beiden auf den Winkel schließe? Das ginge glaube ich nicht...

Kann der RP6 Trigonometrie?

[Fabian E.]

Ohne allzu viel drüber nachzudenken müsste man anhand der gefahren Strecke WÄHREND dem Drehen (auf der Stelle) über ein paar Kreisfunktionen auch den Winkel berechnen können.
Ist mir aber jetzt zu spät dafür ;) du hast ja dann quasi eine Strecke auf einer Kreisbahn, also einen Kreisabschnitt. Damit kann man eigentlich auch den entsprechenden Winkel berechnen.

[fabqu]

Jojo, das geht. Danke Dir!!!
Werd das schon hinbekommen :D

Was ich aber noch nicht weiß: Wie schauts mit Sinus, Cosinus und Tangens aus? Die bräuchte ich um mit Strecke und Winkel einen x- und y-Koordinaten zu berechnen...

Bis denne...
Fabian

[RolfD]

Ich hatte es schon mal angesprochen...
http://www.nongnu.org/avr-libc/user-manual/group__avr__math.html
Die Lib gehört zum gcc

[radbruch]

Hallo


Nun ja, da ich das ganze in einem Raum mit rutschfestem, gleichmäßigem Boden mache, denke ich, dass sich der Schlupf der Ketten gering hält. ... Der Einfachheit halber werde ich zunächst nur geradeaus fahren und mich auf der Stelle Drehen.Vom Ansatz her wohl gar nicht schlecht. Wenn ich mich recht erinnere, dann beträgt die Abweichung von der Geraden bei geregeltem moveAtSpeed() ca. 10 bis 20 cm auf 2m. Ursache dafür ist aber nicht nur der Schlupf, auch die sehr einfache Regelung der Geschwindigkeit ohne Berücksichtigung der Lage spielt da rein. Ein (von mir nicht getesteter) PID-Ansatz wurde hier beschrieben: http://www.arexx.com/forum/viewtopic.php?f=9&t=630&p=5572

Was würde eigentlich passieren, wenn zwischen Antriebsriemen und Boden kein Schlupf mehr auftreten würde? Der RP6 würde schnurrgeradeaus düsen. Eine Kurve oder gar eine Drehung auf der Stelle wäre aber unmöglich. Deshalb brauchen wir den Schlupf zum Drehen, aber leider können wir nicht sagen, wo er auftritt, bzw. wo letzlich der Drehpunkt ist.

Gruß

mic

[Magelan1979]

Ich habe mir mal ein paar Gedanken bezüglich einer speicheroptimierten Lage in der Ebene gemacht. Ich kam dann auf die Idee einfach einen Vektor zu verwenden. Hier mein Erguss

Annahme RP6 ist nach Norden ausgerichtet:

V(x) = (x-Achse, y-Achse, Winkel)

V(0) = (0,0,0)
+ 20 Einheiten geradeaus
V(1) = (0,20,0)
+ Drehung um 20° im UZS und 15 Einheiten geradeaus.
V(2) = V(1) + (sin(20°) * 15, cos(20°) * 15 ,20) ͌ = (5.13, 14.09,20)
+ Drehung um 90° im UZS und 15 Einheiten geradeaus
v(3) = V(2) + (sin(110°) * 15, cos(110°) * 15,110) = (19.22,8.95,110)
+ Drehung um 90° im UZS und 15 Einheiten geradeaus
v(4) = v(3) + (sin(190°) * 15, cos(110°) * 15,190) = (16.61-5.82,190)

Ich hoffe mal, dass meine letzte Geometriestunde nicht alzu lange her ist. Auch wenn Fehler drin sein sollten - bitte melden - sollte der Gedanke klar sein.

Gruß Magelan

[fabqu]

Ah ja, math.h!
Muss ich die noch extra includieren? Wenn ja, wo?

Und das mit der Berechnung mit Sin und Cos... sollte denke ich stimmen, hatte ich mir auch so aufgeschrieben.
Immer schöne mit Ankathete und Gegenkathete :D

Fabian

[RP6conrad]

Meine odometrie ablauf fur eine differential drive ist relatif einfach :
Sobald das e_compass (unterschied linker und rechter tics von antrieb) eine bestimme Schwelle ueberschreitet oder die gefahrene abstand (delta_distance) eine Schwell ueberschreitet wird die neu X und Y position berechnet. Obwohl einfach, functioniert das sehrt gut : Meine robby kan nach 20 curven und Abstanden von 10 meter noch immer zuruck nach das Startpunkt fahren. Aber ich habe keine Schlupf-probleme !! Mit den RP6 konnen sie das vergessen, wegend Schlupf. Abstande gerade aus geht noch, aber curven sind fiel zu ungenau.

void odometrie (void){
int16_t delta_compass; //hoekverdraaing tov vorige doorloop
static int16_t e_compass_old; //waarde e_compass bij vorige doorloop (encodertics)

delta_compass=abs(e_compass - e_compass_old); //hoekverdraaing tov vorige doorloop (encodertics)

if((abs(delta_distance) > 200)||(delta_compass > 100)||(isMovementComplete()&(abs(delta_distance) > 10))){ //alleen update indien voldoende verschil !!
int32_t delta_x = 0; //||((delta_distance>5)&isMovementComplete())
int32_t delta_y = 0;
double hoek= ((e_compass_old+e_compass)/2/E_COMPASS_RESOLUTION)*M_PI/180;//hoek omzetten naar radialen

delta_x = delta_distance*cos(hoek); //
delta_y = delta_distance*sin(hoek); // opgelet overflow vanaf delta_distance >125 !!
delta_distance=0; //resetten van delta_distance ;
x_pos=x_pos+delta_x; //aktuele x_pos updaten *ENCODER_RESOLUTION/2
y_pos=y_pos+delta_y; //aktuele y-pos updaten *ENCODER_RESOLUTION/2
e_compass_old=e_compass;
if(program==10){
writeString("\n");
writeInteger(x_pos,DEC);writeString_P(" ");
writeInteger(y_pos,DEC);writeString_P(" ");
writeInteger(e_compass,DEC);writeString_P(" ");
writeInteger(state,DEC);writeString_P(" ");
}
}

[fabqu]

Hi Conrad,
wenn ich das richtig sehe, dann berechnet dein Programm ununterbrochen während der Fahrt die Koordinaten?
Genau so bruche ich das.
Aber was meinst du mit "Aber ich habe keine Schlupf-probleme !! Mit den RP6 konnen sie das vergessen, wegend Schlupf. Abstande gerade aus geht noch, aber curven sind fiel zu ungenau." ?
Hast du nun Probleme mit deinem RP6, oder wie?

Und was ist "delta_distance"? Woher kommt das?
Und woher kommt "program" ? Wo wird das hochgezählt?
Was mir in deinem Programm ein bisschen fehlt: Der Robby weiß glaube ich nie, in welchem Winkel er steht, oder?

Danke Dir!
Fabian

[RP6conrad]

Diese "odometrie()" function nutzte ich nich in meinen RP6, aber in eine "differential drive" robby. Da ich die RP6 Software wirklich sehr gut finden, nutze ich die als basis für al meine Roboter.
Die odometrie function wird in Hauptschleife angerufen. Sobald eine minimale Abstand, oder eine minimale Winkelaenderung seitens letze Mal das diese Function durchgelaufen war, wird er wieder durchgelaufen.
delta_distance ist das Anzahl tics von L und R motor zusammen seitens letze Durchlauf. Sobald diesen Wert grosse ist dan 200 tics, wird die neue X_pos und Y-pos berechnet
Identisch met delta_compass : alte winkel - actuelle winkel.
Den actuelle winkel wird auch jedes mal in ISR von Encoder neu berechnet (differenz von L und R motor tics). Da den RP6 keine kwadratur encoder hat, muss du da naturlich noch Rechnung halten oder der Motor forwarts oder ruckwarts angesteuert wird.
Die odometrie fucntioniert damit auch in Curven !!
Die variabele Program ist eine globlae variabele, womit ich das Program von Robby wahlen. Nur bei progam nr 10 wird eine Ausdruck gemacht von diese Positionen und Winkel bei jeden Durchlauf.
Hier meine Encoder ISR (bei RP6 fehlt die kwadratur enc !!)

ISR (INT0_vect)
{
if(PINC&KMI_L){
mleft_dist--;
mleft_counter--; //writeString_P("ISR0");//controle interrupt
e_compass--;
delta_distance--;} //richting robby !!

else {
mleft_dist++;
mleft_counter++;
e_compass++;
delta_distance++;} //delta_distance voor odometrie !!

}
/**
* External Interrupt 1 ISR
* (ENCR)
*
*/
ISR (INT1_vect)
{
if(PINC&KMI_R){
mright_dist++;
mright_counter++; //writeString_P("ISR0");//controle interrupt
e_compass--;
delta_distance++;} //richting robby !!
else {
mright_dist--;
mright_counter--;
e_compass++;
delta_distance--;} //delta_distance voor odometrie !!
//delta_distance voor odometrie !!
}

[fabqu]

Hallo!
Erst mal: Leider wurde das Projekt auf die nächste Woche verswchoben...
Aber noch eine Frage dazu:
Die Lib-Befehle mleftDist & mrightDist (oder so ähnlich), wann genau werden die mit Werten gefüllt? Nach jedem "isMovementComplete"? oder ununterbrochen?
Und muss ich die selbst auf '0' zurücksetzen?
Denn ich will ja nach jedem einzelnen Manöver folgendes in ein Array schreiben:
Distanz links | Distanz rechts | gefahrene Strecke geradeaus | gedrehter Winkel | x-Position | y-Position
Und da brauche ich natürlich nur die momentanen Werte...

Danke Euch!
Fabian

[Fabian E.]

Soweit ich mich erinnere werden die fortlaufend befüllt.
Und ja, ich glaube du musst die auch per Hand nullen. Da würde ich mir aber noch mal genauer ansehen, ob das so ohne weiteres möglich ist, oder ob das die Lib eher verwirrt.
Zur Not einfach eine zweite Variable als delta benutzen.

[fabqu]

Ja. Ist einfacher...
Hat jemand eine Idee, wie ich vom Distanzunterschied zwischen linkem und rechtem Encoder auf den gedrehten Winkel komme? Ich verstehe nicht ganz, wie die das in der Lib.c machen.
Hier mal der Auszug, von dem ich glaube, dass er Winkel(unint16_t angle) in Distanzen umrechnet:

void rotate(uint8_t desired_speed, uint8_t dir, uint16_t angle, uint8_t blocking)
{
motion_status.move_L = true;
motion_status.move_R = true;
uint16_t distance = (uint16_t) (((uint32_t)(ROTATION_FACTOR) * (uint16_t)angle)/100);
preDecelerate_L = distance - 100;
preDecelerate_R = distance - 100;
preStop_L = distance;
preStop_R = distance;
if(distance < 40) {
distance = 40;
preStop_L = 20;
preStop_R = 20;
preDecelerate_L = 10;
preDecelerate_R = 10;
}
moveAtSpeed(desired_speed,desired_speed);
changeDirection(dir);

mleft_dist = 0;
mright_dist = 0;
distanceToMove_L = distance;
distanceToMove_R = distance;

motion_status_tmp = motion_status.byte;
MOTIONCONTROL_stateChangedHandler();
if(blocking)
while(!isMovementComplete())
task_RP6System();
}Ich hätts gern andersrum... Winkel in Grad!

mfg

[Fabian E.]

Also entweder machst du es über etwas Mathe mit den Kreisfunktionen (keine Ahung wie genau das wird) oder eben andersherum wie hier beschrieben.
Dazu sollten dann deine Distanzen über beide Ketten etwa gleich groß sein (eventuell das Mittel nehmen)
und dann stellst du dir die Formel da eben nach angle frei und du kannst zurück rechnen.
Ich denke fast, dass das die genauere Lösung sein dürfte, der Author wird sich ja schon was bei dieser Lösung gedacht haben.

[fabqu]

Und was ist der "ROTATION FACTOR"? Bzw. wo steht der?

[Fabian E.]

Och komm schon... ;)
Der wird ja sogar in der Anleitung erwähnt...
Das ist ein define in der RP6Config.h... Im OrdnerRP6common.

[fabqu]

Oha :D
Sorry, in den Commons hab ich noch nciht geguckt :)
Aber danke dir!

[RP6conrad]

mleft_dist und mright_dist werden in den encoder ISR geupdated. Darum wirden die dauernd actualisiert. Aber die werden auch gebraucht in die function move() und rotate(). Wen die auftrag lauft, darfs du naturlich nicht diesen variabelen aendern, oder die move() stimmt nicht mehr !!
Das unterschied von distance_left und distance_right kan einfach in "Graden" umgesetz werden durch dividieren bei eine feste factor : die ROTATIONS_FACTOR. Da macht du einfach das umgekerhte von eine rotate() function.
Das genau ist das problem mit ein "track" roboter : diese ROTATIONS_FACTOR ist sehr abhangig von geschwindigkeit, boden usw. Auf eine dreh von 90° kann das ruhig 20° mehr oder weniger sein. Bei eine differential drive ist das eher +/-1° !!

[Fabian E.]

Da hat RP6conrad vollkommen recht, ich persönlich würde das ganze mit der M32 realisieren und diese Konstante nicht konstant sondern variabel machen.
Die Variable würde ich dann im EEPROM der M32 speichern und bei Bedarf an den RP6 weiter reichen (-->I2C).
Somit könntest du die Variable während der Fahrt per Fernbedienung veränderbar machen und somit den RP6 während der Fahrt "kalibrieren".

Damit könntest du sehr leicht einen passenden Wert herausfinden. Eventuell noch während der Fahrt den Wert im Display anzeigen oder so.

[fabqu]

Klinkgt vernünftig.
Dann wäre es aber sinnvoll, es erst einmal so (mit konstantem Faktor) zu machen, sich in verschiedenen Situationen die Abweichungen anzusehen und dann dementsprechend sich zu überlegen, wie mans Variabel macht, oder???
Gruß

[Fabian E.]

Naja, da du ja deine Logik zum Entgegenehmen von Befehlen via Fernbedienung schon hast würde ich es direkt variabel machen.
Ist aber natürlich jetzt keine Grundsatzentscheidung ;)
Du könntest auch verschiedene "Profile" für verschiedene Untergründe anlegen.