Hrvatsko društvo za robotiku - Uvod u robotiku - vježbe

void RobotMaze::moveTurn() {
	// Note that this function will execute many times during a single turn.
	static uint32_t startedAtMs;
	if (setup())
		startedAtMs = millis();

	int8_t speed = actionMoveTurn->turnByCCW > 180 ? TOP_SPEED : -TOP_SPEED;
	motorGroup->go(speed, -speed); // Turn on the motors. In fact, only first 
		run will do anything as the speeds will not change later.
	...
}

void RobotMaze::moveTurn() {
	...
	if (fabs(mrm_imu->heading() - actionMoveTurn->endAngle) < 5) { 
		motorGroup->stop();
		directionCurrent = actionMove->direction; 
		// Remember the new heading that IMU following algorithm uses. If no 
			wall to alignment after turn, this value will be used immediately.
		imuLastValid = actionMoveTurn->endAngle;
		if (testMode)
			end();
		else
			actionSet(actionDecide); // After rotation, the tile stays
				the same, and no mapping will be needed. The new 
				action is decision what to do next.
	}
}

void RobotMaze::moveTurn() {
	...

	/* A safety measure as timeout elapsed. Most probably, the robot is stuck.
		In this example program we will just quit. In a production program
		evasive actions should be performed, like turning CW-CCW vigorously, or going 
		a little forward. We will have to do whatever possible to rotate
		the robot in the desired heading. Without success, the exit bonus will 
		probably be lost in this point.*/
	if (millis() - startedAtMs > 5000) {
		motorGroup->stop();
		print("Turn timeout %i->%i\n\r", startedAtMs, millis());
		end(); // End of program.
	}
}

void RobotMaze::moveTurnTest(){
	testMode = true; 
	directionCurrent = Direction::UP;
	actionMoveTurn->turnByCCW = 90;
	actionSet(actionMoveTurn);
}