}
			else
			{
				speed_value_tmp = atoi(receiveBuffer);
				if(speed_value_tmp == 0)
				{
					uint8_t not_a_number = 0;
					int8_t i;
					for(i = strlen(receiveBuffer)-1; i >= 0 ; i--)
						if((receiveBuffer[i] < '0') || (receiveBuffer[i] > '9'))
						{
							not_a_number = 1;
							writeString_P("\n### NOT A NUMBER! ###\n");
							break;
						}
					if(not_a_number)
						speed_value_tmp = speed_value;	
				}
				else if(speed_value_tmp > 200){speed_value_tmp = 200;}
				else if(speed_value_tmp < 0) {speed_value_tmp = 0;}
				speed_value = speed_value_tmp;
				
				writeString_P("\n--> Change speed to:");
				writeInteger(speed_value_tmp,DEC);
				writeChar('\n');
							
				moveAtSpeed(speed_value,speed_value);
			}
		}
		if(getStopwatch1() > 250)
		{
			printMotorValues();
			setStopwatch1(0);
		}
		task_RP6System();
	}
	stopStopwatch1();
	mSleep(500);
	setMotorPower(0,0);
	moveAtSpeed(0,0);
	setMotorDir(FWD,FWD);
	powerOFF();
	done();
}

void testMoveDistance(void)
{
	bars(2);
	writeString_P("\nMove Distance test\n");
	writeString_P("#####################################################################\n");
	writeString_P("### ATTENTION!!! DANGER!!! WARNING!!!\n");
	writeString_P("Make sure that the RP6 can __NOT__ move!\n");
	writeString_P("The caterpillar tracks should __NOT__ touch the ground!\n(hold it in your hands for example...)\n");
	writeString_P("THE RP6 CAN START MOVING VERY FAST! YOU CAN DAMAGE IT IF YOU DO NOT\n");
	writeString_P("MAKE SURE THAT IT CAN __NOT__ MOVE!\n");
	writeString_P("#####################################################################\n\n");
	writeString_P("\n\nUsage: Enter Distance values like: \n");
	writeString_P("150, 500, 1580, 4000, 30000 (max!)\n");
	writeString_P("Enter \"end\" or \"x\" and hit return when you want to stop the test!\n");
	writeString_P("Enter \"fwd\", \"bwd\", \"l\" or \"r\" to change direction!\n");
	writeString_P("Enter \"x\" and hit return when you are ready!\n");
	enterX();

	powerON();
	setMotorDir(FWD,FWD);
	setMotorPower(0,0);
	moveAtSpeed(0,0);

	uint16_t dist = 0;
	int16_t dist_tmp = 0;


	while(true)
	{
		if(getInputLine())
		{
			if(strcmp(receiveBuffer,"end")==0)
			{
			    writeString_P("\nEND\n");
				break;
			}
			else if(strcmp(receiveBuffer,"x")==0)
			{
			    writeString_P("\nEND\n");
				break;
			}
			else if(strcmp(receiveBuffer,"fwd")==0)
			{
			    writeString_P("\nChange direction: FWD\n");
				changeDirection(FWD);
			}
			else if(strcmp(receiveBuffer,"bwd")==0)
			{
			    writeString_P("\nChange direction: BWD\n");
				changeDirection(BWD);
			}
			else if(strcmp(receiveBuffer,"l")==0)
			{
			    writeString_P("\nChange direction: Left\n");
				changeDirection(LEFT);
			}
			else if(strcmp(receiveBuffer,"r")==0)
			{
			    writeString_P("\nChange direction: Right\n");
				changeDirection(RIGHT);
			}
			else
			{
				dist_tmp = atoi(receiveBuffer);
				if(dist_tmp == 0)
				{
					uint8_t not_a_number = 0;
					int8_t i;
					for(i = strlen(receiveBuffer)-1; i >= 0 ; i--)
						if((receiveBuffer[i] < '0') || (receiveBuffer[i] > '9'))
						{
							not_a_number = 1;
							writeString_P("\n### NOT A NUMBER! ###\n");
							break;
						}
				}
				else if(dist_tmp < 0) {dist_tmp = 0;}
				dist = dist_tmp;
			
				writeString_P("\n--> Move:");
				writeInteger(dist_tmp,DEC);
				writeChar('\n');
				setStopwatch1(250);
				startStopwatch1();
				move(40,FWD,dist,false);
			}
		}
		if(getStopwatch1() > 250)
		{
			printMotorValues();
			if(isMovementComplete())
			{
				stopStopwatch1();
				mSleep(500);
				task_RP6System();
				printMotorValues();
				writeString("\n--> Movement is complete!\n");
			}
			setStopwatch1(0);
		}
		task_RP6System();
	}
	stopStopwatch1();
	mSleep(500);
	setMotorPower(0,0);
	moveAtSpeed(0,0);
	setMotorDir(FWD,FWD);
	powerOFF();
	done();
}

void testEncoderDutyCycle(void)
{
	bars(2);
	writeString_P("\nEncoder Duty Cycle test\n\n");
	writeString_P("#####################################################################\n");
	writeString_P("### ATTENTION!!! DANGER!!! WARNING!!!\n");
	writeString_P("Make sure that the RP6 can __NOT__ move!\n");
	writeString_P("The caterpillar tracks should __NOT__ touch the ground!\n(hold it in your hands for example...)\n");
	writeString_P("THE RP6 CAN START MOVING VERY FAST! YOU CAN DAMAGE IT IF YOU DO NOT\n");
	writeString_P("MAKE SURE THAT IT CAN __NOT__ MOVE!\n");
	writeString_P("#####################################################################\n\n");
	
	writeString_P("Speed regulation is disabled - the motors get constant Power!\n");
	writeString_P("This allows you to adjust the small potis on the Encoders\n");
	writeString_P("to set the duty cycle properly.\n");
	writeString_P("Optimal is a value of 50% - but if it jumps up and down between\n");
	writeString_P("20% and 80% this is also OK.\n");
	writeString_P("A few \"BAD!!!\" messages (maybe 2 each 5 seconds) are normal and\n");
	writeString_P("nothing to worry about.\n");
	writeString_P("The Software measurement used here is not perfect at all!\n");
	writeString_P("If there are more error messages - you need to adjust the potis a bit\n");
	writeString_P("to make it work!");
	writeString_P("\n\nUsage: Enter PWM values like: \n");
	writeString_P("0, 25, 94, 100, 120 (LIMITED MAXIMUM!!!) and hit Enter! Default value is 55!\n");
	writeString_P("Enter \"end\" or \"x\" and hit return when you want to stop the test!\n");
	writeString_P("Enter \"fwd\", \"bwd\", \"l\" or \"r\" to change direction!\n");
	writeString_P("Enter \"x\" and hit return when you are ready to START!\n");

	enterX();

	uint16_t pwm_tmp = 55;
	uint8_t pwm = 55;

	powerON();
	setMotorDir(FWD,FWD);
	moveAtSpeed(0,0);
	startStopwatch1();
	setMotorPower(pwm,pwm);
	mSleep(500);
	clearReceptionBuffer();
	while(true)
	{
		if(getInputLine())
		{
			if(strcmp(receiveBuffer,"end")==0 || strcmp(receiveBuffer,"x")==0)
			{
			    writeString_P("\nEND\n");
				break;
			}
			else if(strcmp(receiveBuffer,"fwd")==0)
			{
			    writeString_P("\nChange direction: FWD\n");
				changeDirection(FWD);
			}
			else if(strcmp(receiveBuffer,"bwd")==0)
			{
			    writeString_P("\nChange direction: BWD\n");
				changeDirection(BWD);
			}
			else if(strcmp(receiveBuffer,"l")==0)
			{
			    writeString_P("\nChange direction: Left\n");
				changeDirection(LEFT);
			}
			else if(strcmp(receiveBuffer,"r")==0)
			{
			    writeString_P("\nChange direction: Right\n");
				changeDirection(RIGHT);
			}
			else
			{
				pwm_tmp = atoi(receiveBuffer);
				if(pwm_tmp == 0)
				{
					uint8_t not_a_number = 0;
					int8_t i;
					for(i = strlen(receiveBuffer)-1; i >= 0 ; i--)
						if((receiveBuffer[i] < '0') || (receiveBuffer[i] > '9'))
						{
							not_a_number = 1;
							writeString_P("\n### NOT A NUMBER! ###\n");
							break;
						}
					if(not_a_number)
						pwm_tmp = pwm;	
				}
				else if(pwm_tmp > 120){pwm_tmp = 120; writeString_P("\n--> Power limited to 120!");}
				else if(pwm_tmp < 0) {pwm_tmp = 0;}
				pwm = pwm_tmp;
				
				writeString_P("\n--> Change speed to:");
				writeInteger(pwm_tmp,DEC);
				writeChar('\n');
							
				setMotorPower(pwm,pwm);
			}
		}
	
		if(getStopwatch1() > 200)
		{
			if(pwm > 0)
			{
				if(getLeftSpeed() < 10)
				{
					writeString_P("SPEED LEFT IS TOO LOW!!!!\n");
				}
				if(getRightSpeed() < 10)
				{
					writeString_P("SPEED RIGHT IS TOO LOW!!!!\n");
				}
				
				uint16_t cycle = cycle_h_l + cycle_l_l;
				uint8_t dc_high =(uint8_t) ((cycle_h_l*100) / cycle);
				writeString_P("Duty Cycle + LEFT: ");
				writeInteger(dc_high,DEC);
				writeChar('%');
				if(dc_high >= 75 || dc_high < 30)
				{
					writeString_P(" -> BAD !!!");
				}
				else if(dc_high >= 30)
				{
					writeString_P(" -> OK");
				}
			
				cycle = cycle_h_r + cycle_l_r;
				dc_high = (uint8_t) ((cycle_h_r*100) / cycle);
				
				writeString_P(" || Duty Cycle + RIGHT: ");
				writeInteger(dc_high,DEC);
				writeChar('%');
				if(dc_high >= 75 || dc_high < 30)
				{
					writeString_P(" BAD !");
				}
				else if(dc_high >= 30)
				{
					writeString_P(" OK");
				}
				
				writeString_P("  | VL:"); 
				writeInteger(getLeftSpeed(),DEC);
				writeString_P("  | VR:"); 
				writeInteger(getRightSpeed(),DEC);
				writeChar('\n');
			}
			setStopwatch1(0);
		}
	}
	stopStopwatch1();
	mSleep(500);
	setMotorPower(0,0);
	setMotorDir(FWD,FWD);
	powerOFF();
	done();
}

/*****************************************************************************/
// Main:

int main(void)
{
	initRobotBase(); 
	
	uint8_t test = 0;

	setLEDs(0b111111);
	mSleep(500);
	setLEDs(0b000000);
	mSleep(250);
	
	IRCOMM_setRC5DataReadyHandler(receiveRC5Data);
	writeString_P("\n\nRP6 Robot Base Selftest\n\n");
#ifdef FACTORY
	writeString_P("\n\nATTENTION: FACTORY VERSION! \n"); 
	writeString_P("You need to use a laboratory Power supply set to 7.20V!\n\n"); 
#endif
	writeString_P("#####################################################################\n");
	writeString_P("### ATTENTION!!! DANGER!!! WARNING!!!\n");
	writeString_P("Make sure that the RP6 CAN __NOT__ MOVE!\n");
	writeString_P("The caterpillar tracks should NOT touch the ground!\n(hold it in your hands for example...)\n");
	writeString_P("THE RP6 WILL START MOVING IN TEST 8! YOU CAN DAMAGE IT IF YOU DO NOT\n");
	writeString_P("MAKE SURE THAT IT CAN __NOT__ MOVE!\n");
	writeString_P("Make sure both crawler tracks are FREE RUNNING! DO NOT BLOCK THEM!\n");
	writeString_P("#####################################################################\n\n");
	writeString_P("Please enter \"x\" and hit Enter to continue!\n");
	
	writeString_P("Hint: This program requires a newline character (\"\\n\") at the end of\n");
	writeString_P("all input lines!\n");
	clearReceptionBuffer(); 
	enterX();
	clearReceptionBuffer();
	
	while(true)
	{
		// This menu is mainly a program space filler - good for serial interface (speed) testing ;)
		// The same applies for ALL other text information in this program!
		// All this is just to have a BIG program download ;)
		writeChar('\n');
		writeString_P("#####################################################################\n"); 
		writeString_P("#########            RP6 Robot Base Selftest                #########\n"); 
#ifdef FACTORY
		writeString_P("######### FACTORY VERSION (!! 7.2V !!)  v. 1.3 - 25.09.2007 #########\n"); 
#endif
#ifdef HOME
		writeString_P("######### HOME VERSION                  v. 1.3 - 25.09.2007 #########\n"); 
#endif
	    writeString_P("#####################################################################\n"); 
		writeString_P("#####       Main Menu         #########      Advanced Menu      #####\n"); 
		writeString_P("#                                 #                                 #\n"); 
		writeString_P("# 0 - Run ALL Selftests (0-8)     # s - Move at speed Test          #\n"); 
		writeString_P("# 1 - PowerOn Test                # d - Move distance Test          #\n"); 
		writeString_P("# 2 - LED Test                    # c - Encoder Duty-Cycle Test     #\n"); 
		writeString_P("# 3 - Voltage Sensor Test         #                                 #\n"); 
		writeString_P("# 4 - Bumper Test                 #                                 #\n"); 
		writeString_P("# 5 - Light Sensor Test           #                                 #\n"); 
		writeString_P("# 6 - ACS (and RC5 receive) Test  #                                 #\n"); 
		writeString_P("# 7 - IRCOMM/RC5 Test             # System voltage is:              #\n"); 
		writeString_P("# 8 - Motors and Encoders Test    #      "); 
		printUBat(readADC(ADC_BAT));
		writeString_P("                     #\n"); 
		writeString_P("#                                 #                                 #\n"); 
		writeString_P("#####################################################################\n"); 
		writeString_P("# Please enter your choice (1-8, s, d, c)!                          #\n"); 
		writeString_P("#####################################################################\n"); 

		enterString();
	
		test = receiveBuffer[0] - 48;
		
		if(receiveBuffer[0] == 's')
		{
			speed_control();
		}
		else if(receiveBuffer[0] == 'd')
		{
			testMoveDistance();
		}
		else if(receiveBuffer[0] == 'c')
		{
			testEncoderDutyCycle();
		}
		else if(test > 8)
		{
			writeString_P("You need to enter a single number from 0 to 8, s, d or c!");
			continue;
		}
		else
		{
			switch(test)
			{
				case 0: 
					testPowerOn();
					testLEDs();
					testVoltageSensor(); 
					testBumpers(); 
					testLightSensors(); 
					testACS(); 
					testRC5(); 
					testMotorsAndEncoders();
				break; 	
				case 1: testPowerOn(); 				break; 
				case 2: testLEDs(); 				break; 
				case 3: testVoltageSensor();		break; 		
				case 4: testBumpers(); 				break;	
				case 5: testLightSensors(); 		break;
				case 6: testACS(); 			 		break;
				case 7: testRC5(); 		 	 		break;
				case 8: testMotorsAndEncoders(); 	break;	
			}
		}
	}
	return 0;
}