Wegspeicherung

[SH-Robotics]

Hatte jetzt mal wieder Zeit den Code auszuprobieren, aber es gibt ein paar kleine Fehler bei der Ausführung. Manchmal fährt er länger manchmal kürzer(liegt warscheinlich an der stopfunktion, aber wenn ich die in die obere funktion verschiebe geht garnichts mehr(warscheinlich wegen task_RP6System)), machmal fährt er aber auch in eine andere richtung als er eigentlich sollte(sehr selten) und manchmal hört er nichtmehr auf in eine richtung zu fahren...

könnte mir bitte jemand helfen?

#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 rewind = false;


void receiveRC5Data(RC5data_t rc5data)
{
	// Output the received data:
	writeString_P("Toggle Bit:");
	writeChar(rc5data.toggle_bit + '0');
	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:
			pos = pos - 1;
			rewind = true;
			movement_command = 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)
	{
		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) 
	{
		setStopwatch2(0);
		startStopwatch2();
		while(pos > 0 && rewind == true)
		{
			if(getStopwatch2() > 500)
			{
				switch(array[pos])
				{
					case KEY_LEFT:
						speedvar_left = MOVE_FAST;
						speedvar_right = 0;
						changeDirection(BWD);
					break;
					case KEY_RIGHT:
						speedvar_left = 0;
						speedvar_right = MOVE_FAST;
						changeDirection(BWD);
					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;
				task_RP6System();
				stopStopwatch2();
				setStopwatch2(0);
				startStopwatch2();
			}
			
		}
		rewind = false;
		stopfunction();
		task_RP6System();
	}
	return 0;
}