Wegspeicherung

[SH-Robotics]

Ich versuche jetzt mit hilfe einer RC5 Fernbedienung den Roboter zu steuern, was auch funktioniert aber die Wegspeicherung will einfach nicht.
Wenn ich die Rewind taste drücke führt er nur den letzten befähl aus, ich vermute das der Microprozessor das array schneller abarbeitet, als das Fahrwerk den Befehlen nachkommt. Mit der sleep-funktion hab ich wenig erfahrung und sie funktioniert auch nicht so wie ich will.
Erkennt jemand den fehler im Code?

#include "RP6RobotBaseLib.h"

#define KEY_LEFT 				4
#define KEY_RIGHT 				6
#define KEY_FORWARDS 			2
#define KEY_BACKWARDS 			8
#define KEY_STOP 				5
#define KEY_LEFTCURVE 			1
#define KEY_RIGHTCURVE 			3
#define KEY_LEFTCURVEBACK 		7
#define KEY_RIGHTCURVEBACK 		9
#define KEY_REWIND				60


#define MOVE_FAST 200
#define MOVE_SLOW 100

uint8_t speedvar_left;
uint8_t speedvar_right;
uint8_t array[50];
uint8_t pos = 0;
uint8_t tmp = 0;
uint8_t unstopable = false;


void receiveRC5Data(RC5data_t rc5data)
{
	// Output the received data:
	writeString_P("Toggle Bit:");
	writeChar(rc5data.toggle_bit + '0');
	writeString_P(" | Device Address:");
	writeInteger(rc5data.device, DEC);
	writeString_P(" | Key Code:");
	writeInteger(rc5data.key_code, DEC);
	writeChar('\n');
	
	uint8_t movement_command = false;

	switch(rc5data.key_code)
	{
		case KEY_LEFT:
			speedvar_left = 0;
			speedvar_right = MOVE_FAST;
			changeDirection(FWD);
			movement_command = true;
			tmp = 4;
		break;
		case KEY_RIGHT:
			speedvar_left = MOVE_FAST;
			speedvar_right = 0;
			changeDirection(FWD);
			movement_command = true;
			tmp = 6;
		break;
		case KEY_FORWARDS:
			speedvar_left = MOVE_FAST;
			speedvar_right = MOVE_FAST;
			changeDirection(FWD);
			movement_command = true;
			tmp = 2;
		break;
		case KEY_BACKWARDS:
			speedvar_left = MOVE_FAST;
			speedvar_right = MOVE_FAST;
			changeDirection(BWD);
			movement_command = true;
			tmp = 8;
		break;
		case KEY_STOP:
			speedvar_left = 0;
			speedvar_right = 0;
			movement_command = true;
			tmp = 5;
		break;
		case KEY_LEFTCURVE:
			speedvar_left = MOVE_SLOW;
			speedvar_right = MOVE_FAST;
			changeDirection(FWD);
			movement_command = true;
			tmp = 1;
		break;
		case KEY_RIGHTCURVE:
			speedvar_left = MOVE_FAST;
			speedvar_right = MOVE_SLOW;
			changeDirection(FWD);
			movement_command = true;
			tmp = 3;
		break;
		case KEY_LEFTCURVEBACK:
			speedvar_left = MOVE_SLOW;
			speedvar_right = MOVE_FAST;
			changeDirection(BWD);
			movement_command = true;
			tmp = 7;
		break;
		case KEY_RIGHTCURVEBACK:
			speedvar_left = MOVE_FAST;
			speedvar_right = MOVE_SLOW;
			changeDirection(BWD);
			movement_command = true;
			tmp = 9;
		break;
		case KEY_REWIND:
			unstopable = true;
			pos = pos - 1;
			do
			{
				sleep(500);
				switch(array[pos])
				{
					case KEY_LEFT:
						speedvar_left = MOVE_FAST;
						speedvar_right = 0;
						changeDirection(FWD);
					break;
					case KEY_RIGHT:
						speedvar_left = 0;
						speedvar_right = MOVE_FAST;
						changeDirection(FWD);
					break;
					case KEY_FORWARDS:
						speedvar_left = MOVE_FAST;
						speedvar_right = MOVE_FAST;
						changeDirection(BWD);
					break;
					case KEY_BACKWARDS:
						speedvar_left = MOVE_FAST;
						speedvar_right = MOVE_FAST;
						changeDirection(FWD);
					break;
					case KEY_STOP:
					break;
					case KEY_LEFTCURVE:
						speedvar_left = MOVE_FAST;
						speedvar_right = MOVE_SLOW;
						changeDirection(BWD);
					break;
					case KEY_RIGHTCURVE:
						speedvar_left = MOVE_SLOW;
						speedvar_right = MOVE_FAST;
						changeDirection(BWD);
					break;
					case KEY_LEFTCURVEBACK:
						speedvar_left = MOVE_FAST;
						speedvar_right = MOVE_SLOW;
						changeDirection(FWD);
					break;
					case KEY_RIGHTCURVEBACK:
						speedvar_left = MOVE_SLOW;
						speedvar_right = MOVE_FAST;
						changeDirection(FWD);
					break;
				}
				moveAtSpeed(speedvar_left, speedvar_right);
				pos = pos - 1;
			}while(pos > 0);
			movement_command = false;
			unstopable = false;
		break;
		
	}
	
	if(movement_command)
	{
		moveAtSpeed(speedvar_left, speedvar_right);
		array[pos] = tmp;
		pos = pos + 1;
		
	}	
	setStopwatch1(0);
	startStopwatch1();
}


void stopfunction(void)
{
	if(getStopwatch1() > 500 && unstopable == false)
	{
		moveAtSpeed(0, 0);
		stopStopwatch1();
		setStopwatch1(0);
	}
}

int main(void)
{
	initRobotBase(); 
	
	setLEDs(0b111111);
	mSleep(500);	 
	setLEDs(0b000000); 
	powerON();
	
	// Set the RC5 Receive Handler:
	IRCOMM_setRC5DataReadyHandler(receiveRC5Data);
	
	writeString_P("\n * Turn Left: "); 					writeInteger(KEY_LEFT, DEC);
	writeString_P("\n * Turn Right: "); 				writeInteger(KEY_RIGHT, DEC);
	writeString_P("\n * Move Forwards: "); 				writeInteger(KEY_FORWARDS, DEC);
	writeString_P("\n * Move Backwards: "); 			writeInteger(KEY_BACKWARDS, DEC);
	writeString_P("\n * Stop: "); 						writeInteger(KEY_STOP, DEC);
	writeString_P("\n * Move curve left forwards: "); 	writeInteger(KEY_LEFTCURVE, DEC);
	writeString_P("\n * Move curve right forwards: "); 	writeInteger(KEY_RIGHTCURVE, DEC);
	writeString_P("\n * Move curve left backwards: "); 	writeInteger(KEY_LEFTCURVEBACK, DEC);
	writeString_P("\n * Move curve right backwards: "); writeInteger(KEY_RIGHTCURVEBACK, DEC);
	
	while(true) 
	{
		stopfunction();
		task_RP6System(); // Motion Control tasks etc.
	}
	return 0;
}