Multitasking für den Asuro (II)

[Giwo]

So, ich hab mich in der letzten Woche mal wieder in die ganze Asuro-Materie eingearbeitet, und kleine Verbesserungen an der Multitasking-Bibliothek vorgenommen. Mit der AsuroLib 2.6.1 habe ich mich allerdings noch nicht richtig beschäftigt. Ich habe eben mal bei m_a_r_v_i_n angefragt, ob er Interesse an Multitasking in seiner AsuroLib hat.

Aber hier ist erstmal ein weiteres Testprogramm, das mit der original Asuro-Bibliothek arbeitet. Es lässt den Asuro in Schlangenlinien fahren. Bei einer Kollision weicht er ein Stück zurück. Das ganze habe ich mit 4 Prozessen implementiert. Mehr steht in den Kommentaren am Anfang:

/***************************************************************************

*                                                                         *

*   This program is free software; you can redistribute it and/or modify  *

*   it under the terms of the GNU General Public License as published by  *

*   the Free Software Foundation; either version 2 of the License, or     *

*   any later version.                                                    *

***************************************************************************/

/**************************************************************************
* Dieses Programm demonstriert, wie Multitasking zur Robottersteuerung    *
* eingesetzt werden kann. Es kommen hier 4 Tasks zum einsatz:             *
*                                                                         *
* debugTask: Gibt Statusmeldungen ueber die IR-Schnittstelle aus.         *
*                                                                         *
* cruiseTask: Faehrt abwechselnd leichte Links- und Rechtskurven.         *
*                                                                         *
* avoidTask: Entzieht cruiseTask nach einer Kollision die Kontrolle, und  *
*    faehrt ein Stueck nach hinten.                                       *
*                                                                         *
* MAIN_TASK (hauptprogramm): Koordiniert den Zugriff von cruiseTask und   *
*    avoidTask auf die Motoren.                                           *
*                                                                         *
**************************************************************************/

#include "asuro.h"
#include "task.h"
#include <string.h>

// Einen String ausgeben
void SerWriteString(char *outStr) {
	SerWrite(outStr, strlen(outStr));
}

// Eine Zahl ausgeben
void SerWriteInt(int outInt) {
	char outStr[8];
	itoa(outInt, outStr, 10);
	SerWrite(outStr, strlen(outStr));
}

int cruiseLeft, cruiseRight;	// Motorgeschwindigkeit, gesetzt von cruiseTask

int avoidLeft, avoidRight;		// Motorgeschwindigkeit, gesetzt von avoidTask

int avoidControl;       		// Will avoidTask die Kontrolle ueber die 
								// Motoren uebernehmen?

int currentLeft, currentRight;	// Momentane Motorgeschwindigkeit

char switches;					// Die gedrueckten Taster

//Setzt die Motorgeschwindigkeit. Zulaessige Werte: -255..255 enspricht rueckwerts..vorwaerts
void MyMotorSpeed(int left, int right) {
	char left_dir = left >=0 ? FWD : RWD;
	char right_dir = right >=0 ? FWD : RWD;
	left = left < 0 ? -left : left;
	right = right < 0 ? -right : right;
	
	MotorDir(left_dir, right_dir);
	MotorSpeed(left, right);
}

//Faehrt abwechselnd leichte Links- und Rechtskurven.
void cruiseTask(void) {
	while (1) {
		cruiseLeft=180;
		cruiseRight=200;
		Sleep(72UL*1000); // 1s links herum;
		cruiseLeft=200;
		cruiseRight=180;
		Sleep(72UL*1000); // 1s rechts herum;
	}
}

//Entzieht cruiseTask nach einer Kollision die Kontrolle, und faehrt ein Stueck nach hinten.
void avoidTask(void) {
	while(1) {
		switches = PollSwitch();
		if (switches!=0) {					// Ein Taster geddueckt?
			avoidControl = 1;				// Ja: Kontrolle uebernehmen
			if ((switches & 0x07) != 0) {	// Taster auf der rechten Seite gedrueckt?
				avoidLeft = -150;			// Ja: nach hinten-links ausweichen
				avoidRight = -100;
			} else {
				avoidLeft = -100;			// Sonst: nach hinten-rechts ausweichen
				avoidRight = -150;
			}
			Sleep(72UL*1000);				// 1s nach hinten fahren
			avoidControl = 0;				// Kontrolle wieder an cruiseTask abgeben
		}
		switchTask();
	}
}

//Gibt Statusmeldungen ueber die IR-Schnittstelle aus.
void debugTask(void) {
	while(1) {
		// Motorgeschwindigkeiten
		SerWriteInt(currentLeft);
		SerWriteString(" ");
		SerWriteInt(currentRight);
		SerWriteString(" ");
		
		// Kontrolle
		SerWriteInt(avoidControl);
		SerWriteString(" ");
		
		// Taster
		SerWriteInt(switches);
		SerWriteString(" ");
		
		// Wieviel ist noch auf den Stacks frei?
		SerWriteInt(freeStackSize(1));
		SerWriteString(" ");
		SerWriteInt(freeStackSize(2));
		SerWriteString(" ");
		SerWriteInt(freeStackSize(3));
		
		SerWriteString("\r\n");
		Sleep(75UL*100);
	}
}


//Koordiniert den Zugriff von cruiseTask und avoidTask auf die Motoren.
int main(void)

{
	Init();		/* Asuro initialisieren */
	initTask();	/* Task initialisieren */
	
	cruiseLeft=0;
	cruiseRight=0;
	avoidLeft=0;
	avoidRight=0;
	avoidControl=0;
	
	// Tasks starten
	// Die Stackgroessen wurden experimentell ermittelt. Das kann etwas muesehlig sein,
	// denn: Stack zu klein -> absturtz
	// und: Stack zu gruss -> absturtz
	// Eine Hilfe bietet hier die Funktion freeStackSize, die ermittelt wieviel Platz
	// noch auf einem Stack ist. (Siehe debugTask)
	addTask(cruiseTask, 128);
	addTask(avoidTask, 128);
	addTask(debugTask, 330);
	

	while(1) {
		if (avoidControl) {				// Will avoidTask die Kontrolle haben?
			currentLeft=avoidLeft;		// Ja: avoidTask bekommt Kontrolle
			currentRight=avoidRight;
		} else {
			currentLeft=cruiseLeft;		// sonst: cruiseTask bekommt Kontrolle
			currentRight=cruiseRight;
		}
		MyMotorSpeed(currentLeft, currentRight);
		switchTask();
	}

	return 0;

}

Das ganze funktioniert soweit. Allerdings hat sich herausgestellt, das 1kb Ram recht wenig ist. Die richtigen Stackgrößen für die einzelnen Tasks herauszufinden war etwas Fummelarbeit. Mehr als 4-5 Tasks werden wohl kaum möglich sein.

Das ganze kann man hier runterladen: http://stud.fh-wedel.de/~ii4984/Task.cruise.zip