Wegspeicherung

[SH-Robotics]

ok hier mit Stopwatches, funktioniert aber trotzdem noch nicht.

@Pr0gm4n: Welchen Thread meinst du denn?

#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[10];
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(" | 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;
			setStopwatch2(0);
			startStopwatch2();
			do
			{
				if(getStopwatch2() > 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;
				
					stopStopwatch2();
					setStopwatch2(0);
					startStopwatch2();
				}
				
			}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;
}