RCK Ruđera Boškovića - mobilna / uslužna robotika

Biblioteke - Pokretanje robota

Zadatak

Proučiti biblioteku za pokretanje robota i naučiti kako ju mijenjati.

Priprema

Za vježbu je potrebno:

Datoteke

U ovom prikazu ćemo se koristiti datotekama "mrm-board.h" i "mrm-board.cpp", koje se nalaze u biblioteci "mrm-board".

Biblioteku možemo uređivati iz Arduino IDE-a, na način koji je opisan u prethodnim vježbama.

MotorGroup

class MotorGroup {
protected:
	MotorBoard* motorBoard[MAX_MOTORS_IN_GROUP] = { NULL, NULL, NULL, NULL }; // Motor board for each wheel. It can the same, but need not be.
	uint8_t motorNumber[MAX_MOTORS_IN_GROUP];
	Robot* robotContainer;

	/** Angle between -180 and 180 degrees
	@return - angle
	*/
	float angleNormalized(float angle);
public:
	MotorGroup(Robot* robot);

	/** Stop motors
	*/
	void stop();
};
Način pogona robota je definiran objektno.

Bazna klasa je "MotorGroup" i nalazi se u "mrm-board.h".

MotorGroupDifferential deklaracija

/** Motor group for tank-like propulsion.
*/
class MotorGroupDifferential : public MotorGroup {
private:
	/** Check if speed is inside bounds
	@param speed - speed to be checked
	@return - speed inside bounds
	*/
	int16_t checkBounds(int16_t speed);

public:
	/** Constructor
	@param motorBoardForLeft1 - Controller for one of the left wheels
	@param motorNumberForLeft1 - Controller's output number
	@param motorBoardForRight1 - Controller for one of the right wheels
	@param motorNumberForRight1 - Controller's output number
	@param motorBoardForLeft2 - Controller for one of the left wheels
	@param motorNumberForLeft2 - Controller's output number
	@param motorBoardForRight2 - Controller for one of the right wheels
	@param motorNumberForRight2 - Controller's output number
	*/
	MotorGroupDifferential(Robot* robot, MotorBoard* motorBoardForLeft1, uint8_t motorNumberForLeft1, MotorBoard* motorBoardForRight1, uint8_t motorNumberForRight1,
		MotorBoard* motorBoardForLeft2 = NULL, uint8_t motorNumberForLeft2 = 0, MotorBoard* motorBoardForRight2 = NULL, uint8_t motorNumberForRight2 = 0);

	/** Start all motors
	@param leftSpeed, in range -127 to 127
	@param right Speed, in range -127 to 127
	@param speedLimit - Speed limit, 0 to 127. For example, 80 will limit all the speeds to 80/127%. 0 will turn the motors off.
	*/
	void go(int16_t leftSpeed = 0, int16_t rightSpeed = 0, int16_t lateralSpeedToRight = 0, uint8_t speedLimit = 127);
};
Dalje ćemo raditi na primjeru robota s diferencijalnim pogonom.

Lijevi kod je i dalje iz datoteke "mrm-board.h".

Klasa kojom je predstavljen ovaj pogon je "MotorGroupDifferential" i izvedena je iz "MotorGroup".

Lijevo je deklarirana funkcija "checkBounds()" koja provjerava jesu li parametri u okviru dozvoljenog i njom se nećemo baviti.

U javnom dijelu ("public") se nalazi konstruktor funkcija "go()", kojom ćemo se baviti najviše.

Konstruktor (MotorGroupDifferential()) omogućava da se bilo koji kontroler motora i bilo koji broj motora na njemu spoji na određeni kotač s lijeve ili desne strane.

Zašto funkcija "go()" nije bila deklarirana u baznoj klasi? Zato što je pogon za svaku vrstu pogonske grupe dovoljno različit da ga ne može pokriti jedna funkcija. Zato svaka izvedena klasa ima svoju "go()" funkciju.

MotorGroupDifferential definicija - 1

/** Start all motors
@param leftSpeed, in range -127 to 127
@param right Speed, in range -127 to 127
@param speedLimit - Speed limit, 0 to 127. For example, 80 will limit all the speeds to 80/127%. 0 will turn the motors off.
*/
void MotorGroupDifferential::go(int16_t leftSpeed, int16_t rightSpeed, int16_t lateralSpeedToRight, uint8_t speedLimit) {
	for (uint8_t i = 0; i < 4; i++)
		if (motorBoard[i] == NULL)
			return;

	...
}
Otvorimo "mrm-board.cpp".

U ovoj je datoteci definirana funkcija "go()", čiju smo deklaraciju vidjeli gore.

Proučimo na koji način.

Na početku, ako nije za svaki motor definiran njegov kontroler (nego je vrijednost NULL), odmah se izlazi iz funkcije.

MotorGroupDifferential definicija - 2

/** Start all motors
@param leftSpeed, in range -127 to 127
@param right Speed, in range -127 to 127
@param speedLimit - Speed limit, 0 to 127. For example, 80 will limit all the speeds to 80/127%. 0 will turn the motors off.
*/
void MotorGroupDifferential::go(int16_t leftSpeed, int16_t rightSpeed, int16_t lateralSpeedToRight, uint8_t speedLimit) {
	for (uint8_t i = 0; i < 4; i++)
		if (motorBoard[i] == NULL)
			return;

	if (speedLimit == 0)
		stop();
	else {
		if (speedLimit > 127)
			speedLimit = 127;
		...
	}
}
Ako smo ograničili brzinu na 0, odmah zaustavljamo motore.

Ako je tražena brzina preko maksimalne, smanjujemo na maksimalnu.

MotorGroupDifferential definicija - 3

/** Start all motors
@param leftSpeed, in range -127 to 127
@param right Speed, in range -127 to 127
@param speedLimit - Speed limit, 0 to 127. For example, 80 will limit all the speeds to 80/127%. 0 will turn the motors off.
*/
void MotorGroupDifferential::go(int16_t leftSpeed, int16_t rightSpeed, int16_t lateralSpeedToRight, uint8_t speedLimit) {
	for (uint8_t i = 0; i < 4; i++)
		if (motorBoard[i] == NULL)
			return;

	if (speedLimit == 0)
		stop();
	else {
		if (speedLimit > 127)
			speedLimit = 127;
		int16_t speeds[4];
		speeds[0] = checkBounds(leftSpeed - lateralSpeedToRight);
		speeds[1] = checkBounds(-rightSpeed + lateralSpeedToRight);
		speeds[2] = checkBounds(leftSpeed - lateralSpeedToRight);
		speeds[3] = checkBounds(-rightSpeed + lateralSpeedToRight);
		...
	}
}
Deklariramo polje od 4 elementa u kojima ćemo računati brzine motora.

Lijeve motore ćemo pokrenuti brzinom za lijeve, desnom za desne.

Elementi 0 i 2 polja "speed" će biti lijevi motori, 1 i 3 desni.

Pribrojimo lateralno gibanje, ako robot ima omni-kotače i može se kretati postrance.

Svaki motor propustimo kroz funkciju koja pazi da su brzine unutar ograničenja.

MotorGroupDifferential definicija - 4

/** Start all motors
@param leftSpeed, in range -127 to 127
@param right Speed, in range -127 to 127
@param speedLimit - Speed limit, 0 to 127. For example, 80 will limit all the speeds to 80/127%. 0 will turn the motors off.
*/
void MotorGroupDifferential::go(int16_t leftSpeed, int16_t rightSpeed, int16_t lateralSpeedToRight, uint8_t speedLimit) {
	for (uint8_t i = 0; i < 4; i++)
		if (motorBoard[i] == NULL)
			return;

	if (speedLimit == 0)
		stop();
	else {
		if (speedLimit > 127)
			speedLimit = 127;
		int16_t speeds[4];
		speeds[0] = checkBounds(leftSpeed - lateralSpeedToRight);
		speeds[1] = checkBounds(-rightSpeed + lateralSpeedToRight);
		speeds[2] = checkBounds(leftSpeed - lateralSpeedToRight);
		speeds[3] = checkBounds(-rightSpeed + lateralSpeedToRight);
		float maxSpeed = abs(speeds[0]);
		for (int i = 1; i < 4; i++)
			if (abs(speeds[i]) > maxSpeed)
				maxSpeed = abs(speeds[i]);
		...
	}
}
Izračunamo maksimalnu apsolutnu brzinu.

MotorGroupDifferential definicija - 5

/** Start all motors
@param leftSpeed, in range -127 to 127
@param right Speed, in range -127 to 127
@param speedLimit - Speed limit, 0 to 127. For example, 80 will limit all the speeds to 80/127%. 0 will turn the motors off.
*/
void MotorGroupDifferential::go(int16_t leftSpeed, int16_t rightSpeed, int16_t lateralSpeedToRight, uint8_t speedLimit) {
	for (uint8_t i = 0; i < 4; i++)
		if (motorBoard[i] == NULL)
			return;

	if (speedLimit == 0)
		stop();
	else {
		if (speedLimit > 127)
			speedLimit = 127;
		int16_t speeds[4];
		speeds[0] = checkBounds(leftSpeed - lateralSpeedToRight);
		speeds[1] = checkBounds(-rightSpeed + lateralSpeedToRight);
		speeds[2] = checkBounds(leftSpeed - lateralSpeedToRight);
		speeds[3] = checkBounds(-rightSpeed + lateralSpeedToRight);
		float maxSpeed = abs(speeds[0]);
		for (int i = 1; i < 4; i++)
			if (abs(speeds[i]) > maxSpeed)
				maxSpeed = abs(speeds[i]);
		for (uint8_t i = 0; i < 4; i++) {
			if (maxSpeed > speedLimit) 
				motorBoard[i]->speedSet(motorNumber[i], (int8_t)(speeds[i] / maxSpeed * speedLimit));
			else
				motorBoard[i]->speedSet(motorNumber[i], (int8_t)speeds[i]);
		}
	}
}
Pokrenemo sva 4 motora.

Motori se pokreću funkcijom "speedSet()", koju smo već upoznali.

Ako je maksimalna brzina ograničena, smanjimo brzine.

Primjedbe