rp6base_selftest.c

RP6机器人范例程序。包括移动

	{
		if(getInputLine())
			break;
			
		task_ACS();

		static uint8_t info_left = false;
		static uint8_t info_right = false;
	
		if(isObstacleLeft()) 
		{
			if(!info_left) 
			{			 
				writeString_P("OBSTACLE: LEFT!\n");
				info_left = true;
			}
		}
		else if(info_left) 
		{
			writeString_P("FREE: LEFT!\n");
			info_left = false;
		}
		if(isObstacleRight()) 
		{
			if(!info_right) 
			{
				writeString_P("OBSTACLE: RIGHT!\n");
				info_right = true;
			}
		}
		else if(info_right) 
		{
			writeString_P("FREE: RIGHT!\n");
			info_right = false;
		}

		statusLEDs.byte = 0b000000;
		if(isObstacleLeft() && isObstacleRight())
			statusLEDs.byte = 0b100100;
		statusLEDs.LED5 = isObstacleLeft();
		statusLEDs.LED2 = isObstacleRight();
		statusLEDs.LED4 = (!isObstacleLeft());
		statusLEDs.LED1 = (!isObstacleRight());
		updateStatusLEDs();
	}
	setACSPwrOff();
	powerOFF();
	setLEDs(0b000000);
	done();
}

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

void testRC5(void)
{
	test(7);
	writeString_P("\nIRCOMM Test\n");
	writeString_P("This test will transmit several RC5 Codes with the IRCOMM and check\n");
	writeString_P("if they are received back properly!\n");
	writeString_P("Please make sure that there is a white ceiling or some reflective\n");
	writeString_P("Object directly over the IRCOMM LEDs on the front of the RP6!\n\n");
	writeString_P("Please DO NOT send anything with TV Remote Controls or similar\n");
	writeString_P("as this will disturb the test and it will fail!\n\n");
	writeString_P("The ACS is active and the Robot will detect obstacles that you\nmove in front of it!\n\n");
#ifdef HOME
	writeString_P("\n### Enter \"x\" and hit return to START this test!\n\n");
	enterX();
#endif
	mSleep(500);
	powerON();
	setACSPwrOff();
	startStopwatch1();
	setLEDs(0b001000);
	RC5_data = 0;
	RC5_error = 0;
	RC5_test = 0;
	uint8_t errors = 0;
	uint8_t i = 0;
	while(true)
	{
		if(getStopwatch1() > 500) 
		{		
			if(RC5_test==2)
			{
				writeString_P("\nTIMEOUT!!!\n\n");
				errors = true;
				break;
			}
			writeString_P("TX RC5 Packet: ");
			writeInteger(i,DEC);
			writeChar('\n');
			
			IRCOMM_sendRC5(i, i);
			RC5_data = i;
			RC5_test = 2;
			i+=3;
			setStopwatch1(0);
		}
		if(RC5_test==10)
		{
			RC5_test = 0;
			setStopwatch1(450);
			if(i > 63)
			{
				writeString_P("\nTest finished!\n");
				break;
			}
		}
		if(RC5_test==5)
		{
			RC5_test = 0;
			writeString_P("\nRECEPTION ERROR!!!\n\n");
			errors = true;
			break;
		}
		
		task_ACS();
	}
	
	if(errors)
	{
		setLEDs(0b110110);
		writeString_P("####### !!!  WARNING WARNING WARNING  !!! #######\n");
		writeString_P("####### !!! TEST FINISHED WITH ERRORS !!! #######\n");
		writeString_P("####### !! Please check IRCOMM assembly ! #######\n");
		writeString_P("\n\nEnter any character to continue!\n");
		setACSPwrOff();
		powerOFF();
		enterString();
	}
	
	setACSPwrOff();
	stopStopwatch1();
	powerOFF();
	mSleep(1000);
	setLEDs(0b000000);
	done();
}

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

void testMotorsAndEncoders(void)
{
	test(8);
	writeString_P("\nAutomatic speed speed regulation 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 WILL START MOVING FAST! 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("--> OTHERWISE THE TEST WILL FAIL!\n");
	writeString_P("#####################################################################\n\n");
#ifdef HOME
	writeString_P("### Enter \"x\" and hit return when TO START this test!\n### Make sure the RP6 can not move!\n\n");
	enterX();
	if(receiveBuffer[0]!='x')
		return;
#endif
	task_RP6System();

	powerON();
	setMotorDir(FWD,FWD);
	setMotorPower(0,0);
	moveAtSpeed(0,0);
	setStopwatch1(0);
	setStopwatch2(0);
	startStopwatch1();
	startStopwatch2();
	uint8_t state = 0;
	uint8_t cnt = 0;
	uint8_t speed_test = 0;
	uint8_t errors = 0;
	task_RP6System();
	while(true)
	{
		if(getStopwatch2() > 400)
		{
			writeString_P("T: ");
			writeIntegerLength(speed_test,DEC,3);
			writeString_P(" |VL: ");
			writeIntegerLength(getLeftSpeed(),DEC,3);
			writeString_P(" |VR: ");
			writeIntegerLength(getRightSpeed(),DEC,3);
			writeString_P(" |PL: ");
			writeIntegerLength(getMotorLeft(),DEC,3);
			writeString_P(" |PR: ");
			writeIntegerLength(getMotorRight(),DEC,3);
			writeString_P(" |IL: ");
			writeIntegerLength(adcMotorCurrentLeft,DEC,3);
			writeString_P(" |IR: ");
			writeIntegerLength(adcMotorCurrentRight,DEC,3);
			writeString_P(" |UB: ");
			printUBat(adcBat);
			writeChar('\n');
			if((getLeftSpeed() < 130 && adcMotorCurrentLeft > 120)
			   || (getLeftSpeed() >= 130 && adcMotorCurrentLeft > 150))
			{
				writeString_P("####### Left: CURRENT CONSUMPTION IS TOO HIGH! ########\n");
				errors++;
			}
			if((getRightSpeed() < 130 && adcMotorCurrentRight > 120)
			   || (getRightSpeed() >= 130 && adcMotorCurrentRight > 150))
			{
				writeString_P("####### Right: CURRENT CONSUMPTION IS TOO HIGH! ########\n");
				errors++;
			}
			
			if(getLeftSpeed() > 25 && getMotorLeft() >= 30 && adcMotorCurrentLeft < 10)
			{
				writeString_P("####### Left: Current Sensor is defect!!! Current measurement is too low! ########\n");
				errors++;
			}
			if(getRightSpeed() > 25 && getMotorRight() >= 30 && adcMotorCurrentRight < 10)
			{
					writeString_P("####### Right: Current Sensor is defect!!! Current measurement is too low! ########\n");
				errors++;
			}
#ifdef HOME
			if(adcBat < 550)
			{
				writeString_P("####### ERROR: Supply Voltage is too low (<5.50V)! Please check power supply! #######\n");
				errors++;
			}
#endif
#ifdef FACTORY
			if(adcBat < 650)
			{
				writeString_P("####### ERROR: Supply Voltage is too low (<6.50V)! Please check power supply! #######\n");
				errors++;
			}
#endif
			if(errors)
				break;

			setStopwatch2(0);
		}
		if(getStopwatch1() > 1000)
		{
				moveAtSpeed(speed_test,speed_test);
				if(cnt > 2)
				{
					if((getLeftSpeed() >= ((speed_test > 5) ? (speed_test - 10) : speed_test)) 
						&& (getLeftSpeed() <= (speed_test+10)))
					{
						writeString_P("Speed Left: OK\n");
					}
					else
					{
						writeString_P("####### ERROR Left ########\n");
						errors++;
					}

					if((getRightSpeed() >= ((speed_test > 5) ? (speed_test - 10) : speed_test)) 
						&& (getRightSpeed() <= (speed_test+10)))
					{
						writeString_P("Speed Right: OK\n");
					}
					else
					{
						writeString_P("####### ERROR Right #######\n");
						errors++;
					}
#ifdef FACTORY
					if(getMotorLeft() > (3*getLeftSpeed()))
					{
						writeString_P("####### ERROR: LEFT PWM Value is too high! Check motor assembly! #######\n");
						errors++;
						break;
					}
					if(getMotorRight() > (3*getRightSpeed()))
					{
						writeString_P("####### ERROR: LEFT PWM Value is too high! Check motor assembly! #######\n");
						errors++;
						break;
					}
#endif
					if(errors) break;

					if(state == 1)
					{
						if(speed_test <= 120)
						{
							if(speed_test >= 80)
								changeDirection(BWD);
							else if(speed_test >= 60)
								changeDirection(FWD);
							else if(speed_test >= 40)
								changeDirection(BWD);
							else if(speed_test < 40)
								state = 2;
						}
						speed_test -= 20;
#ifdef FACTORY
						if(speed_test >= 120)
							speed_test -= 20;
#endif
					}

					if(state == 0)
					{
#ifdef FACTORY
						if(speed_test >= 160)
							state = 1;
						if(speed_test >= 80)
							speed_test += 20;
#endif
#ifdef HOME
						if(speed_test >= 80)
							state = 1;
#endif
						speed_test += 20;
					}
					cnt = 0;
				}
				else
					cnt++;
			setStopwatch1(0);
		}
		task_RP6System();

		if(state==2)
		{
			moveAtSpeed(0,0);
			setMotorPower(0,0);
			break;
		}
		task_RP6System();
	}

	stopStopwatch1();
	setMotorPower(0,0);
	moveAtSpeed(0,0);
	setMotorDir(FWD,FWD);
	powerOFF();
	if(errors)
	{
		setLEDs(0b110110);
		writeString_P("####### !!!  WARNING WARNING WARNING  !!! #######\n");
		writeString_P("####### !!! TEST FINISHED WITH ERRORS !!! #######\n");
		writeString_P("#### Please check motor and encoder assembly! ###\n");
		writeString_P("#### And also check Sensors for defects!      ###\n");
		writeString_P("\n\nEnter any character to continue!\n");
		powerOFF();
		enterString();
	}
	else
	{
		writeString_P("\n***** MOTOR AND ENCODER TEST OK! *****\n");
		mSleep(1000);
	}
	setLEDs(0b000000);
}

void printMotorValues(void)
{
	writeString_P("PL: ");
	writeIntegerLength(getMotorLeft(),DEC,3);
	writeString_P(" | PR: "); 
	writeIntegerLength(getMotorRight(),DEC,3);
	writeString_P(" | IL: ");
	writeIntegerLength(adcMotorCurrentLeft,DEC,3);
	writeString_P(" | IR: ");
	writeIntegerLength(adcMotorCurrentRight,DEC,3);
	writeString_P(" | VL: ");
	writeIntegerLength(getLeftSpeed(),DEC,3);
	writeString_P(" | VR: ");
	writeIntegerLength(getRightSpeed(),DEC,3);
	writeChar('\n');
	writeString_P(" | DSTR: ");
	writeInteger(getRightDistance(),DEC);
	writeString_P(" | DSTL: ");
	writeInteger(getLeftDistance(),DEC);
	writeString_P(" ###  | UBAT: "); 
	printUBat(adcBat);
	writeChar('\n');
}

void speed_control(void)
{
	bars(2);
	writeString_P("\nManual speed regulation 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("\n\nUsage: Enter Speed values like: \n");
	writeString_P("0, 25, 94, 100, 155, 200 (max!) and hit Enter!\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();
	clearReceptionBuffer();

	powerON();
	setMotorDir(FWD,FWD);
	setMotorPower(0,0);
	moveAtSpeed(0,0);
	uint8_t speed_value = 0;
	int16_t speed_value_tmp = 0;
	startStopwatch1();
	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);