Selvkørendeog selvorienterende robot

Rasmus har bygget sin selvkørende robot til at kunne “se sig for” og tilpasse sin retning i forhold til omgivelserne.
Skulle robotten overse en forhindring har den tryk sensorer i fronten som den kan reagere på.

[lyte id=”ImpAHO6HC6A” /]

#pragma config(Sensor, S1,     sensor,         sensorSONAR)
#pragma config(Sensor, S2,     touch1,         sensorTouch)
#pragma config(Sensor, S3,     touch2,         sensorTouch)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task scan() // This task makes the sensor go back and forth
{
	string direction = "right";

	while(true)
	{

		if(SensorValue[sensor] > 80)
		{

			if(nMotorEncoder[motorA] > 40)
			{
				direction = "left";
			}
			if(nMotorEncoder[motorA] < -40)
			{
				direction = "right";
			}

			if (direction == "left") motor[motorA] = -10;
			if (direction == "right") motor[motorA] = 10;

		}
		else
		{
			motor[motorA] = 0;
		}
	}
}

task main()
{
	nMotorEncoder[motorA] = 0;
	StartTask(scan);
	while(true)
	{

		int power;
		power = SensorValue[sensor];
		power = (power-40);

		if(SensorValue[touch1] == 1)
		{
			PlaySound(soundUpwardTones);
			motor[motorB] = -100;
			motor[motorC] = 0;
			wait1Msec(1000);
		}

		if(SensorValue[touch2] == 1)
		{
			PlaySound(soundUpwardTones);
			motor[motorB] = 0;
			motor[motorC] = -100;
			wait1Msec(1000);
		}

		if((SensorValue[touch1] == 0) && (SensorValue[touch2] == 0))
		{

			if(SensorValue[sensor] < 80)
			{
				if(nMotorEncoder[motorA] < 0)
				{
					motor[motorB] = power;
					motor[motorC] = 100;
				}
				if(nMotorEncoder[motorA] > 0)
				{
					motor[motorB] = 100;
					motor[motorC] = power;
				}
			}
			else
			{
				motor[motorB] = 100;
				motor[motorC] = 100;
			}

		}
	}
}

Skriv et svar