Reaali Robootika.COM

NXT robotimaailm ja programmeerimine C-keeles

PID-kontrolleri ja kompassi abil robot otse sõitma

17.veebruari tund möödus puhtalt selle tähe all, et kuidas ehitada PuckCollect robotile süsteem kompassi ja PID-kontrolleri abil otsesõitmiseks.

Rao tuli päris keerulise matemaatilise loogikaga lagedale, mille peale mul lihtsalt aju suitsema hakkas ja ma mõtlesin excelis välja sama asja ainult et lihtsamal moel. Õnneks leidis Rao sellest ka väikese vea, enne kui robotisse programmeerima hakkas.

Seega kompassi ja  PID-kontrolleriga otsesõitmine. Tegelikult kasutame ainult P-kontrolleri osas, sellest piisab meie robotile.

task PID()
{
	int compass;
	float error;
	int turn;
	int speed1;
	int speed2;
	float  Kp = 5;
	compass = SensorHTCompass(S3);
while(1)
{
		if(MindSensorTouch(S4, 2))
			break;
		error = compass - SensorHTCompass(S3);
		if(error < -180)
				error += 360;
		if(error > 180)
				error -= 360;
		turn = error * Kp;
		speed1 = BAASKIIRUS + turn;
		speed2 = BAASKIIRUS - turn;
		if(speed1 > 100)
			speed1 = 100;
		if(speed1 < -100)
			speed1 = -100;
		if(speed2 > 100)
			speed2 = 100;
		if(speed2 < -100)
			speed2 = -100;
		
		OnFwd(OUT_B, speed2);
		OnFwd(OUT_C, speed1);
}  	
Off(OUT_BC);
}

Robot ise näeb täna juba välja selline:

IMG_1360

Add comment

Loading