“Line tracker” Lego Mindstorms robot med stabiliserende funktion.

Med denne funktion vil robotten have mindre udsving ved små afvigelser og eksponentiel reaktion på større afvigelser.

[lyte id=”2wyDVKY3CBg” /]

 

#pragma config(Sensor, S1,     light,          sensorLightActive)
#pragma config(Sensor, S2,     sound,          sensorSoundDB)
#pragma config(Sensor, S4,     eye,            sensorSONAR)
#pragma config(Motor,  motorB,          right,         tmotorNXT, PIDControl, driveRight, encoder)
#pragma config(Motor,  motorC,          left,          tmotorNXT, PIDControl, driveLeft, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


// int Threshold = 35;  White paper = 40-41, Blue Tape = 24-25

task main()
{
	int lightValue;
	int Threshold = 35; // 29
	float speedChange;
	int motorSpeed = 40; // 30
	int factor;

	displayCenteredTextLine(2, "%s","Sensor Value:");

	factor = (motorSpeed / 10);

	while(true)
	{
		lightValue = SensorValue[light]; // Displays sensor value
		displayCenteredTextLine(5, "%d",lightValue);


		if(SensorValue[light]<Threshold)
		{
			speedChange = (Threshold - SensorValue[light]);
			speedChange = (speedChange = ((pow(speedChange, 3)) * 0.02));
			speedChange = (speedChange * factor);
			motor(left) = (motorSpeed - round(speedChange));
		}
		else
		{
			motor(left) = motorSpeed;
		}

		if(SensorValue[light]>Threshold)
		{
			speedChange = (SensorValue[light] - Threshold);
			speedChange = (speedChange = ((pow(speedChange, 2.5)) * 0.05)); 
			speedChange = (speedChange * factor);
			motor(right) = (motorSpeed - round(speedChange));
		}
		else
		{
			motor(right) = motorSpeed;
		}

		wait1Msec(10);

	}

}

 

Se logikken i den stabiliserende funktion – klik for større billede.

balancing robot stabiliserings funktion
balancing robot stabiliserings funktion

Skriv et svar