Subsumption Architektur

**ehenkes** · 15.06.2007, 23:30

Mit Ultraschallecholot funktioniert das auch recht gut:

Code:

#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** · 16.06.2007, 20:51

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

Code:

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** · 16.06.2007, 22:24

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:

Code:

#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** · 16.06.2007, 22:40

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

Code:

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

**ehenkes** · 16.06.2007, 22:58

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** · 16.06.2007, 23:20

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** · 17.06.2007, 00:01

OK, mein Code sieht momentan so aus:

Code:

#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** · 17.06.2007, 13:36

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:

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** · 17.06.2007, 14:08

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** · 19.06.2007, 23:26

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.

Code:

#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);		
}

Thema: Subsumption Architektur

Themen-Optionen

Thema durchsuchen

Anzeige

Lesezeichen

Lesezeichen

Berechtigungen