Subsumption Architektur

[matoge]

Hallo zusammen

ich habe jetzt die ersten Fingerübungen mit dem Asuro erfolgreich hinter mich gebracht und mir vorgenommen, die Programmierung im Stil der Subsumption Architektur von Rodney Brooks zu machen.

sieh z.B http://de.wikipedia.org/wiki/Rodney_A._Brooks

Hat da schon jemand Erfahrungen mit dem Asuro gesammelt?

Bin sehr gespannt

Manfred

[damaltor]

=) viel glück... irgendwo im forum geistert eine möglichkeit, den asuro multitasking-fähig zu machen. vielleicht hilft dir das dabei.

[Osser]

Hi matoge,

dachte immer dass es Addsumption heisst.

Man lernt nie aus... :)

Bin gerade mit einem Multitasking Projekt beschäftigt für den ATmega8L um mehrere Tasks laufen zu lassen und mit Treibern die Peripherie auszulesen und zur Verfügung zu stellen.

Darauf aufbauend kann recht einfach die Subsumption von Brooks aufgebaut werden. Wobei dass eigentlich nicht unbedingt nötig ist, da eine Statemachine auch durchaus gute Dienste leisten kann. Per MT ist es aber wesentlich leichter um durchzublicken, gerade mit grösseren Projekten.

Wenn ich damit soweit bin, dass es einigermassen läuft, werd ich das Projekt im forum posten, dann kannst Du es ja mal damit probieren.


O.

[m.a.r.v.i.n]

Hi,

der Programm Code des c't-Bots ist z.B. so aufgebaut. Das wäre auch für den Asuro machbar. Allerdings ist das Programierung von solchen Verhalten nicht ganz trivial. Den c't-Bot Code gibt es auf dem Heise Server zum Download.
http://www.heise.de/ct/ftp/projekte/ct-bot/download.shtml

[matoge]

Hi matoge,

Bin gerade mit einem Multitasking Projekt beschäftigt für den ATmega8L um mehrere Tasks laufen zu lassen und mit Treibern die Peripherie auszulesen und zur Verfügung zu stellen.

Wenn ich damit soweit bin, dass es einigermassen läuft, werd ich das Projekt im forum posten, dann kannst Du es ja mal damit probieren.


O.

Hi Osser,

im Moment habe ich auch die Hoffnung, ohne Multitasking auszukommen (Overhead). Die Idee, die Peripherie über Treiber auszulesen klingt interessant, bin auf Deine Ergebnisse sehr gespannt.
Wenn ich was lauffähiges vorzuweisen habe, werde ich es auch gerne hier zur Verfügung stellen.

Manfred

[Osser]

Hi matoge,

mal schau'n wann ich das soweit fertig habe. Multitasking läuft bereits, aber das Treibermodell ist noch nicht fertig.
Werde nur ein oder zwei Treiber schreiben (habe leider nie Zeit!), dann kann hier im Forum ja der ein oder andere vielleicht die Fehlenden Peripherietreiber ergänzen.

Stay tuned.

O.

[Osser]

Hey m.a.r.v.i.n,

aber das Reitzt den wahren Freak ja gerade -- stimts.... ;)

O.

[m.a.r.v.i.n]

Hi,

@Osser stimmt natürlich

hier noch was zum Thema Peripherie Treiber. RN-User Rakke hatte schon vor gut 2 Jahren eine Funktion geposted in der alle A/D Wandler per Interrupt abgefragt wurden. Der Trick dabei war, die Odo Sensoren öfters abzufragen als die anderen Sensoren. Damit hätte man die ganze Problematik mit Multitasking Programmen schon mal entschärft, da nur noch auf globale Variablen zugegriffen wird und nicht mehr auf die Peripherie.

Hier der Link zum Download:
https://www.roboternetz.de/phpBB2/download.php?id=5106

Siehe auch folgende Threads:
https://www.roboternetz.de/phpBB2/zeigebeitrag.php?t=7571
https://www.roboternetz.de/phpBB2/zeigebeitrag.php?t=15709

[Osser]

Hi m.a.r.v.i.n,

Erstmal Danke für die Links.

Mit den Treibern will ich auf jedenfall auch auf global deklarierte Bezeichner zurückgreifen, da das meines Erachtens nach den geringstmöglichen Overhead produziert. Semaphoren und Mutexe sind auch nur minimalistisch angedacht, schliesslich muss das Ganze ja mit Treibern in maximal 1-2 KB passen (momentan ca. 1,4KB).

Um die Intervalle der Treiber zu steuern, will ich lediglich auf die Priorisierung der Tasks zurückgreifen, d.h.

hohe Priorität => hohe Abtastfrequenz;
mittlere Priorität => mittlere Abtastfrequenz;
usw.

Natürlich garantiert dass keine RT Abfragen da ich preemtives MT einsetze, sollte aber nicht weiter schlimm sein denke ich.

Mit den modularen Treibern haben wir dann nämlich den entscheidenden Vorteil, dass abhängig von dem was gebraucht wird der entsprechende Treiber gelinkt wird oder eben nicht und dass spart wiederum Platz.


Gruss,

O.

[Osser]

matoge, m.a.r.v.i.n,

hab das angekündigte Multitasking in einer Alpha Release im Thread https://www.roboternetz.de/phpBB2/viewtopic.php?t=27463 gepostet.

Hat momentan eine Overhead von ca. 2KB (weniger als die asuro lib) da der Peripherieteil anders aufgebaut ist und momentan noch nicht alle Treiber implementiert sind. Aber bei den Treibern vertraue ich hier auf die Community, da wird hoffentlich schon bald mehr zur Verfügung stehen.

Naja, schaut's euch mal an.

Tschö.

O.

[rossir]

Subsumption geht auch, relativ kompakt, ohne multi threading oder state transition Technik.

Hier mal mein Beispiel Code, mit dem sich ASURO wacker schlägt.

#include "asuro.h"

typedef int (*FunctionPointer) (int);
FunctionPointer actionList[];

int slow=60;
int fast=80;
unsigned char currentTask;

int wait(int msTime) {
long t1=Gettime()+msTime;
unsigned char newTask, savedTask=currentTask;
int sensor, action=0;

do {
for(newTask=0; newTask<savedTask; ) {
sensor=(*actionList[2*newTask])(newTask);
if(sensor) {
currentTask=newTask;
action|=(*actionList[2*newTask+1])(sensor);
newTask=0;
} else newTask++;
}
} while(t1 > Gettime());
currentTask=savedTask;
return action;
}

void drive(int leftSpeed_, int rightSpeed_) {
SetMotorPower(leftSpeed_, rightSpeed_);
}

int blink_SecTimer(int idx) {
static int t1=0;
int t0=t1;
t1=(Gettime()/1000)&0x1;
return t0^t1; // bei Sekundenwechsel ==> 1 sonst 0
}

int blink_Action(int sensor) {
static int led=GREEN;
led=(led==GREEN) ? RED : GREEN;
StatusLED(led);
return 0x0;
}

int avoid_Obstacle(int idx) {
return PollSwitch();
}

int avoid_Action(int sensor) {
drive(-slow, -slow); // uups erst mal 2 sec zurueck
wait(2000);
if(sensor<16) drive(-slow, 0); else drive(0, -slow); // dreh dich weg vom Hindernis
wait(1000);
return 0x1;
}

int cruise_Forever(int idx) {
return 1;
}

int cruise_Action(int sensor) {
drive(fast, fast); // fahr rum
return 0x8;
}

FunctionPointer actionList[] ={
/* sense , action */
//motor_Awake, motor_Control,
blink_SecTimer, blink_Action,
avoid_Obstacle, avoid_Action,
cruise_Forever, cruise_Action,
};

int main(void) {
Init();
EncoderInit();
EncoderStart();

drive(0,0);
mSleep(1000);
currentTask=sizeof(actionList)/sizeof(FunctionPointer)/2;
return wait(0);
}

ASURO wird dadurch wie folgt gesteuert:
Task blink prio0 (höchste): Die Statuslampe blinkt im Sekundentakt.
Task avoid prio1: ASURO weicht bei einer Kollision aus.
Task cruise prio2 (niedrigste): ASURO fährt grade aus.
Jeder Task besteht aus zwei C-Funktionen.
a) Einem Sensor-Teil der entscheidet ob der Action-Teil ausgeführt werden soll.
b) Einem Action-Teil der die entsprechende Aktion durchführt.

Läuft ein Task hoher Priorität blockiert er solange alle Tasks niedrigerer Priorität bis er fertig ist (= seine Action-Funktion verlassen wird).

Ein Task ist kooperativ und gibt höher priorisierten Task die Gelegenheit zur Ausführung indem er (möglichst) oft die Funktion wait(napTime) aufruft.

Die Koordination des Ganzen liegt in der Hand der Funktion wait(napTime). Diese prüft ob ein Task laufen will (dazu führt sie seine Sensor-Funktion aus) und startet dann gegebenenfalls seine Action-Funktion. Tasks werden immer streng nach Priorität ausgeführt (erinnert mich ein bischen an das Spiel: die Türme von Hanoi).

Beliebig viele Tasks (mit ihrem Sensor- und Action-Teil) werden in das Array actionList[] mit abnehmender Priorität eingestellt.

[ehenkes]

Danke für das interessante Programm. ASURO fährt damit zwar ständig rückwärts und ignoriert auch bei Vorzeichenwechsel Kollisionen, aber das bekommt man schon noch hin.

[damaltor]

kann das an deinen motoren liegen? die sind doch standardmäßig falschrum eingebaut. aber hattest du die nicht schon umgedreht?

[ehenkes]

Mit Go(...) und Turn(...) laufen diese normal. Warum soll ich diese dann umdrehen?

[robo.fr]

Die Subsumption Architektur kann man auch noch einfacher realiesieren ( Mutitasking Beispiele hier (http://www.roboterclub-freiburg.de/asuro/asuro_index.html) )
( siehe auch "Multitasking mit endlichen Automaten")

[rossir]

@ehenkes
Habe das Gefühl das PollSwitch() bei Dir dauernd "anspringt".

Debug doch bitte mal:

1) Zeigt ASURO mit folgendem Ersatzcode das gleiche, von Dir beschriebene, Verhalten?

int avoid_Obstacle(int idx) {
return 1;
}

2) Fährt ASURO mit folgendem Ersatzcode dauernd vorwärts (cruise)?

int avoid_Obstacle(int idx) {
return 0;
}

3) Falls die Anworten "JA" waren, könnte folgender Ersatzcode bei PollSwitch() zu stabileren Ergebnissen führen.

int avoid_Obstacle(int idx) {
static unsigned char count=0;
static int old=0;
int data=PollSwitch();

count++;
if (old!=data) count=0;
old=data;
return (count>4) ? data : 0;
}

4) Und, überhaupt, blinkt denn wenigstens die Statuslampe im Sekundentakt?

@robo.fr
Ist ja auch nur als weitere Alternative gedacht. Empfinde es aber als schöner wenn im Task kein:
"state=5;"
oder
"if (time_in_this_state < timeout_3) {...}"
vom eigentlichen Code der Taskaufgabe ablenkt.

[ehenkes]

Die Status-LED blinkt sehr schön. :)
Ob das genau eine Sekunde ist, habe ich noch nicht überprüft.

Danke! Punkt 3) hat entscheidend geholfen. Jetzt zeigt er bereits "Pantoffeltierchen-Verhalten". Ich werde das auf einen meiner ASUROs mit Ultraschallerweiterung übertragen und weiter verfeinern.

[ehenkes]

Noch zur Erläuterung, um was es hier überhaupt geht:
Die verhaltens-basierte Architektur von Prof. Rodney Brooks (Arbeitsgruppe für Mobile Roboter am MIT) wird auch beim c't-Bot von heise verwendet (siehe:
http://www.henkessoft.de/Roboter/Roboter.htm#ct-Bot_und_ct-Sim ).

Die Grundideen sind folgende:
- Sensorzustände aktivieren Verhalten
- Prioritätenstruktur
- höherwertiges Verhalten kann niederwertiges unterdrücken
- kein geometrisches Weltmodell
- komplexe Umwelt ==> komplexes Verhalten

Hoffnung und Vermutung: Intelligente komplexe Verhalten entstehen durch sehr einfache, reflexive Verhalten

[m.a.r.v.i.n]

Hi,

Einen sehr empfehlenswerten Artikel zur Subsumptions Architektur gibt es auch von David P. Anderson.
Als pdf (http://geology.heroy.smu.edu/~dpa-www/robo/subsumption/subsumption.pdf) und in HTML (http://www.dprg.org/articles/2007-03a/).

[damaltor]

Mit Go(...) und Turn(...) laufen diese normal. Warum soll ich diese dann umdrehen?

hätte ja sein können dass die bei dir ohnehin noch falschrum laufen... hätte mich aber auch stark gewundert =)

[ehenkes]

Mit Ultraschallecholot funktioniert das auch recht gut:


#include "asuro.h"

typedef int (*FunctionPointer) (int);
FunctionPointer actionList[];

int slow=60;
int fast=80;
unsigned char currentTask;

int wait(int msTime)
{
long t1=Gettime()+msTime;
unsigned char newTask, savedTask=currentTask;
int sensor, action=0;

do {
for(newTask=0; newTask<savedTask; )
{
sensor=(*actionList[2*newTask])(newTask);
if(sensor)
{
currentTask=newTask;
action|=(*actionList[2*newTask+1])(sensor);
newTask=0;
}
else ++newTask;
}
} while(t1 > Gettime());
currentTask=savedTask;
return action;
}

void drive(int leftSpeed_, int rightSpeed_)
{
SetMotorPower(leftSpeed_, rightSpeed_);
}

int blink_SecTimer(int idx)
{
static int t1=0;
int t0=t1;
t1=(Gettime()/1000)&0x1;
return t0^t1; // bei Sekundenwechsel ==> 1 sonst 0
}

int blink_Action(int sensor)
{
static int led=GREEN;
if(led==GREEN)
led=RED;
else
led=GREEN;
StatusLED(led);
return 0x0;
}

int avoid_Obstacle(int idx)
{
int abstand = Chirp();
if (abstand<10)
{
BackLED(ON,ON);
return abstand;
}
else
{
BackLED(OFF,OFF);
return 0;
}
}

int avoid_Action(int sensor)
{
static int flag;
drive(-slow, -slow); // 0,5 sec zurueck
wait(500);
if(flag)
{
drive(-slow, 0);
flag=0;
}
else
{
drive(0, -slow);
flag=1;
}
wait(1000); // 1 sec Rückwaertskurve (abwechselnd links und rechts)
return 0x1;
}

int cruise_Forever(int idx)
{
return 1;
}

int cruise_Action(int sensor)
{
drive(fast, fast); // fahr rum
return 0x8;
}

FunctionPointer actionList[] =
{
/* sense , action */
//motor_Awake, motor_Control,
blink_SecTimer, blink_Action,
avoid_Obstacle, avoid_Action,
cruise_Forever, cruise_Action,
};

int main(void)
{
Init();
EncoderInit();
EncoderStart();

drive(0,0);
Msleep(1000);
currentTask=sizeof(actionList)/sizeof(FunctionPointer)/2;
return wait(0);
}

[rossir]

Man kann ASURO bei Allem - also recht modular - damit sogar geregelt (hier einfacher P-Regler) fahren lassen:

int leftSpeed=0;
int rightSpeed=0;
long motorTime=0;
int delta=10, Kp=20; // entspricht Kp=0.2

void drive(int leftSpeed_, int rightSpeed_) {
leftSpeed=leftSpeed_;
rightSpeed=rightSpeed_;
}

int regeln(int pwm, int e) {
int y= (Kp * e)/100;
y= (y>10)? 10 : ((y<-10) ? -10 : y);
pwm+=y;
return (pwm>127)? 127 : ((pwm<-127) ? -127 : pwm);
}

int motor_Awake(int idx) {
return Gettime()>motorTime;
}

int motor_Control(int sensor) {
static int lpwm=0;
static int rpwm=0;
int l_ticks, r_ticks;
int Ta=Gettime()-motorTime;

l_ticks=encoder[LEFT];
r_ticks=encoder[RIGHT];
EncoderReset();
motorTime=Gettime()+delta;

lpwm=regeln(lpwm, leftSpeed -800L*L_DIR*l_ticks/Ta);
rpwm=regeln(rpwm, rightSpeed-800L*L_DIR*r_ticks/Ta);

SetMotorPower(lpwm, rpwm);
return 0x1;
}

und dann später

FunctionPointer actionList[] ={
/* sense , action */
motor_Awake, motor_Control,
...
cruise_Forever, cruise_Action,
};

[ehenkes]

Wofür steht bitte L_DIR? Ich nehme einfach mal 6. ;-)
EncoderReset steht wohl für EncoderSet(0,0).

Mit diesem Programm zeigt er schon ein recht interessantes Verhalten, das allerdings noch bezüglich Ultraschallecholot-Hinderniserkennung und Reaktion optimiert werden muss:


#include "asuro.h"
#define L_DIR 6

typedef int (*FunctionPointer) (int);
FunctionPointer actionList[];

int slow=60;
int fast=80;
unsigned char currentTask;

int leftSpeed=0;
int rightSpeed=0;
long motorTime=0;
int delta=10, Kp=20; // entspricht Kp=0.2



int wait(int msTime)
{
long t1=Gettime()+msTime;
unsigned char newTask, savedTask=currentTask;
int sensor, action=0;

do
{
for(newTask=0; newTask<savedTask; )
{
sensor=(*actionList[2*newTask])(newTask);
if(sensor)
{
currentTask=newTask;
action|=(*actionList[2*newTask+1])(sensor);
newTask=0;
}
else ++newTask;
}
}
while(t1 > Gettime());

currentTask=savedTask;
return action;
}

void drive(int leftSpeed_, int rightSpeed_)
{
leftSpeed = leftSpeed_;
rightSpeed = rightSpeed_;
}

int regeln(int pwm, int e)
{
int y = (Kp * e)/100;
y= (y>10) ? 10 : ((y<-10) ? -10 : y);
pwm += y;
return (pwm>127) ? 127 : ((pwm<-127) ? -127 : pwm);
}

int blink_SecTimer(int idx)
{
static int t1=0;
int t0=t1;
t1 = (Gettime()/1000) & 0x1;
return t0^t1; // bei Sekundenwechsel ==> 1 sonst 0
}

int blink_Action(int sensor)
{
static int led=GREEN;
if(led==GREEN)
led=RED;
else
led=GREEN;
StatusLED(led);
return 0x0;
}

int motor_Awake(int idx)
{
return Gettime()>motorTime;
}

int motor_Control(int sensor)
{
static int lpwm=0;
static int rpwm=0;
int l_ticks, r_ticks;
int Ta = Gettime() - motorTime;

l_ticks = encoder[LEFT];
r_ticks = encoder[RIGHT];
EncoderSet(0,0); //Encoder Reset
motorTime = Gettime() + delta;

lpwm = regeln(lpwm, leftSpeed -800L*L_DIR*l_ticks/Ta);
rpwm = regeln(rpwm, rightSpeed -800L*L_DIR*r_ticks/Ta);

SetMotorPower(lpwm, rpwm);
return 0x1;
}

int avoid_Obstacle(int idx)
{
int abstand = Chirp();
if (abstand<10)
{
BackLED(ON,ON);
return abstand;
}
else
{
BackLED(OFF,OFF);
return 0;
}
}

int avoid_Action(int sensor)
{
static int flag;
drive(-slow, -slow); // 0,5 sec zurueck
wait(500);
if(flag)
{
drive(-slow, 0);
flag=0;
}
else
{
drive(0, -slow);
flag=1;
}
wait(1000); // 1 sec Rückwaertskurve (abwechselnd links und rechts)
return 0x2;
}

int cruise_Forever(int idx)
{
return 1;
}

int cruise_Action(int sensor)
{
drive(fast, fast); // fahr rum
return 0x3;
}

FunctionPointer actionList[] =
{
/* sense , action */
blink_SecTimer, blink_Action,
motor_Awake, motor_Control,
avoid_Obstacle, avoid_Action,
cruise_Forever, cruise_Action,
};

int main(void)
{
Init();
EncoderInit();
EncoderStart();

drive(0,0);
Msleep(1000);
currentTask=sizeof(actionList)/sizeof(FunctionPointer)/2;
return wait(0);
}

[rossir]

Uuups, da habe ich folgendes Code Stückchen vergessen zu liefern.

#define L_DIR ((PORTD & RWD) ? -1 : 1)
#define R_DIR ((PORTB & RWD) ? -1 : 1)

[ehenkes]

lpwm = regeln(lpwm, leftSpeed -800L*L_DIR*l_ticks/Ta);
rpwm = regeln(rpwm, rightSpeed -800L*R_DIR*r_ticks/Ta); //R_DIR korrekt ??
Wieso 800 ?

[rossir]

Wieso 800 ?
Ist nur ein Scaling, mit der Idee, dass
--> SetMotorPower(left right); // ungeregelt
ca. die selbe tatsächliche ASURO Geschwindigkeit liefert wie
--> drive(left, right); //geregelt

Ja, Du hast Recht, es sollte
--> rpwm = regeln(rpwm, rightSpeed -800L*R_DIR*r_ticks/Ta); //R_DIR !!
heißen.

[ehenkes]

OK, mein Code sieht momentan so aus:


#include "asuro.h"
#define L_DIR ((PORTD & RWD) ? -1 : 1)
#define R_DIR ((PORTB & RWD) ? -1 : 1)

typedef int (*FunctionPointer) (int);
FunctionPointer actionList[];

int slow=60;
int fast=80;
unsigned char currentTask;

int leftSpeed=0;
int rightSpeed=0;
long motorTime=0;
int delta=10, Kp=20; // entspricht Kp=0.2

int wait(int msTime)
{
long t1=Gettime()+msTime;
unsigned char newTask, savedTask=currentTask;
int sensor, action=0;

do
{
for(newTask=0; newTask<savedTask; )
{
sensor=(*actionList[2*newTask])(newTask);
if(sensor)
{
currentTask=newTask;
action|=(*actionList[2*newTask+1])(sensor);
newTask=0;
}
else ++newTask;
}
}
while(t1 > Gettime());

currentTask=savedTask;
return action;
}

void drive(int leftSpeed_, int rightSpeed_)
{
leftSpeed = leftSpeed_;
rightSpeed = rightSpeed_;
}

int regeln(int pwm, int e)
{
int y = (Kp * e)/100;
y= (y>10) ? 10 : ((y<-10) ? -10 : y);
pwm += y;
return (pwm>127) ? 127 : ((pwm<-127) ? -127 : pwm);
}

int blink_SecTimer(int idx)
{
static int t1=0;
int t0=t1;
t1 = (Gettime()/1000) & 0x1;
return t0^t1; // bei Sekundenwechsel ==> 1 sonst 0
}

int blink_Action(int sensor)
{
static int led=GREEN;
if(led==GREEN)
led=RED;
else
led=GREEN;
StatusLED(led);
return 0x0;
}

int motor_Awake(int idx)
{
return Gettime()>motorTime;
}

int motor_Control(int sensor)
{
static int lpwm=0;
static int rpwm=0;
int l_ticks, r_ticks;
int Ta = Gettime() - motorTime;

l_ticks = encoder[LEFT];
r_ticks = encoder[RIGHT];
EncoderSet(0,0); //Encoder Reset
motorTime = Gettime() + delta;

lpwm = regeln(lpwm, leftSpeed -8000L*L_DIR*l_ticks/Ta);
rpwm = regeln(rpwm, rightSpeed -8000L*R_DIR*r_ticks/Ta);

SetMotorPower(lpwm, rpwm);
return 0x1;
}

int avoid_Obstacle(int idx)
{
int abstand = Chirp();
if (abstand<10)
{
BackLED(ON,ON);
return abstand;
}
else
{
BackLED(OFF,OFF);
return 0;
}
}

int avoid_Action(int sensor)
{
static int flag;
drive(-slow, -slow); // 0,5 sec zurueck
wait(500);
if(flag)
{
drive(-slow, 0);
flag=0;
}
else
{
drive(0, -slow);
flag=1;
}
wait(200); // 0,2 sec Rückwaertskurve (abwechselnd links und rechts)
return 0x2;
}

int cruise_Forever(int idx)
{
return 1;
}

int cruise_Action(int sensor)
{
drive(fast, fast); // fahr rum
return 0x3;
}

FunctionPointer actionList[] =
{
/* sense , action */
blink_SecTimer, blink_Action ,
motor_Awake , motor_Control,
avoid_Obstacle, avoid_Action ,
cruise_Forever, cruise_Action,
};

int main(void)
{
Init();
EncoderInit();
EncoderStart();

drive(0,0);
Msleep(1000);
currentTask=sizeof(actionList)/sizeof(FunctionPointer)/2;
return wait(0);
}

Die 8000 habe ich genommen, um die Priorität höher zu setzen.
Das erscheint mir momentan noch das Problem. Wie steuert man nachvollziehbar die Priorität. Im Programmgerüst des c't-Bot konnte man diese konkret angeben. Vielleicht kannst Du meinen Code mal testen. Wo würdest Du ansetzen, um ihn weiter zu optimieren?

[rossir]

Irgendwie liegt hier ein Mißverständnis vor. Ob 800 oder 8000, das hat gar nichts mit der Taskpriorität zu tun, sondern ist allein ein motor_Control(..) interner Scaling Faktor für die Encoderticks pro Zeiteinheit.

Ich hätte deshalb erwartet, als ich Deinen Code testete, dass ASURO durch die Verwendung von 8000 (statt 800) ca. 10 mal langsamer fährt. Dem war aber nicht so! Nach einigem Debuggen habe ich den Fehler in motor_Control(..) gefunden. Ta wurde falsch berechnet und deshalb war die Reglung eher chaotisch ;-) und leider auch abhängig von den Nachbartasks. Hier der korrigierte Code:

int delta=100, Kp=20; // entspricht Kp=0.2

int regel(int pwm, int e) {
int y= (Kp * e)/100;
pwm+=y;
return (pwm>127)? 127 : ((pwm<-127) ? -127 : pwm);
}

int motor_Awake(int idx) {
return Gettime()>motorTime;
}

int motor_Control(int sensor) {
static int lpwm=0;
static int rpwm=0;
int l_ticks, r_ticks;
int Ta=Gettime()-motorTime+delta;

l_ticks=encoder[LEFT];
r_ticks=encoder[RIGHT];
EncoderSet(0,0); //Encoder Reset ();
motorTime=Gettime()+delta;

lpwm=regel(lpwm, leftSpeed -1240*L_DIR*l_ticks/Ta);
rpwm=regel(rpwm, rightSpeed-1240*R_DIR*r_ticks/Ta);

SetMotorPower(lpwm, rpwm);
return 0x1;
}
Die Änderungen im Einzelnen:
1) Der Hauptfehler ist in motor_Control(..) durch folgende Zeile korrigiert:
--> int Ta=Gettime()-motorTime+delta;

2) Dadurch ist folgende Zeile in regeln(..) nicht mehr nötig:
--> y= (y>10)? 10 : ((y<-10) ? -10 : y);

3) Ausserdem kommen in 10ms nur 0 oder 1 Encoderticks zustande. Deshalb habe ich delta jetzt auf 100ms gesetzt.

4) Schlußendlich habe ich das Scaling auf 1240 ausgepingelt. (Für die 4-fach Encoderscheibe.)


Eine Gegenfrage: Für welchen unserer Task würde es Sinn machen die Priorität "nachvollziehbar" zu steuern?

Der c't-Bot fährt multi threaded, da macht es Sinn ein wenig - aber nicht zu viel ;-) - über thread Prioritäten nachzudenken.

Bei meinem Programmgerüst wird die Priorität allein durch die Reihenfolge der Einträge in actionList[] festgelegt. Vielleicht sollte man in diesem Fall besser von Tasklayern sprechen statt von Taskprioritäten. (Stichwort: Türme von Hanoi)

"weiter zu optimieren?" Klar, das geht bestimmt immer.

[ehenkes]

Danke. Welchem Wert würde deine Optimierung bei einer 6-fach Encoderscheibe entsprechen? Vielleicht kann man einen Faktor als #define ENCODERSEGMENTS 4 oder #define ENCODERSEGMENTS 6 in das Programm einbauen.

[rossir]

weiter zu optimieren?
Wenn man auf die Aufteilung zwischen Sensor- und Action-Teil in zwei Funktionen verzichtet und beides in eine Funktion packt geht es kompakter.
Das mag bei den knappen ASURO Resourcen sinnvoll sein.

#include "asuro.h"

#define L_DIR ((PORTD & RWD) ? -1 : 1)
#define R_DIR ((PORTB & RWD) ? -1 : 1)
#define ENCODERSEGMENTS 4

typedef int (*FunctionPointer) (int);
FunctionPointer actionList[];

const int slow=60, fast=80;
char currentTask;

int leftSpeed=0;
int rightSpeed=0;

int wait(int msTime) {
long t1=Gettime()+msTime;
char savedTask=currentTask;
int act, action=0;

do {
for(currentTask=0; currentTask<savedTask; currentTask++) {
act=(*actionList[currentTask])(currentTask);
if(act) {
action|=act;
currentTask=-1;
};
}
} while(t1 > Gettime());
currentTask=savedTask;
return action;
}

void drive(int leftSpeed_, int rightSpeed_) {
leftSpeed=leftSpeed_;
rightSpeed=rightSpeed_;
}

const int delta=100, Kp=20; // entspricht Kp=0.2

int regel(int pwm, int e) {
int y= (Kp * e)/100;
pwm+=y;
return (pwm>127)? 127 : ((pwm<-127) ? -127 : pwm);
}

int motor_Task(int idx) {
static long motorTime=0;
static int lpwm=0;
static int rpwm=0;
int l_ticks, r_ticks;
int Ta=Gettime()-motorTime;
if(Ta<0) return 0;

Ta+=delta;

l_ticks=encoder[LEFT];
r_ticks=encoder[RIGHT];
EncoderSet(0, 0);
motorTime=Gettime()+delta;

lpwm=regel(lpwm, leftSpeed -(4960/ENCODERSEGMENTS)*L_DIR*l_ticks/Ta);
rpwm=regel(rpwm, rightSpeed-(4960/ENCODERSEGMENTS)*R_DIR*r_ticks/Ta);

SetMotorPower(lpwm, rpwm);
return 0x1;
}

int blink_Task(int idx) {
static int led=0;
static int t1=0;
int t0=t1;
t1=(Gettime()/1000)&0x1;
if(t0==t1) return 0;

led=1-led;
StatusLED(led);
return 0x1;
}

int avoid_Task(int idx) {
static unsigned char count=0;
static int old=0;
int sensor=PollSwitch();

count++;
if (old!=sensor) count=0;
old=sensor;
if (count<5 || sensor==0) return 0;

drive(-slow, -slow);
wait(2000);
if(sensor<16) drive(-slow, 0); else drive(0, -slow);
wait(1000);
return 0x2;
}

int cruise_Task(int idx) {
drive(fast, fast);
return 0x1;
}

FunctionPointer actionList[] ={
motor_Task,
blink_Task,
avoid_Task,
//findLight_Task,
cruise_Task,
};

int main(void) {
Init();
EncoderInit();
EncoderStart();

drive(0,0);
mSleep(1000);
currentTask=sizeof(actionList)/sizeof(FunctionPointer);
return wait(0);
}