CraftDuino v2.0
  • - это CraftDuino - наш вариант полностью Arduino-совместимой платы.
  • CraftDuino - настоящий конструктор, для очень быстрого прототипирования и реализации идей.
  • Любая возможность автоматизировать что-то с лёгкостью реализуется с CraftDuino!
Просто добавьте CraftDuino!

LEGO Mindstorms. Snatcher. Часть-2

Одной из особенностей контроллеров LEGO Mindstorms NXT является поддержка параллельного выполнения нескольких процессов – многозадачности. То есть робота можно запрограммировать на одновременное выполнение нескольких действий. Далее я покажу как это может помочь при написании программ для роботов.


Поскольку датчик-гироскоп измеряет угловую скорость, то для преобразования показаний, необходимо произвести интегрирование показаний по времени.Но для этого надо постоянно отслеживать показания датчика, а выполнять это в основном цикле программы накладно. Поэтому код обработки показаний датчика-гироскопа выделяется в отдельную задачу, которая называется драйвером гироскопа. Ниже представлен пример такого драйвера


float 	gyroAngle = 0.0;
float 	gyroBias = 0.0;

task GyroDriver()
{
	float gyroVelocity;
	long samplesSum = 0;

	for (int i = 0; i < 100 ; i++)
	{
		samplesSum += SensorRaw[gyroPort];
		wait1Msec(10);
	}
	gyroBias = (float) samplesSum / 100;
	while (true){
		gyroVelocity = SensorValue[gyroPort] - gyroBias;
		gyroAngle += gyroVelocity * 0.01;
	}
}


Контроллеры LEGO Mindstorms NXT поддерживают одновременное выполнение до 10 задач. Ресурсы процессора распределяются между задачами, для каждой из которых устанавливается время, в течение которого задача будет активна.

Для поворота робота на заданный угол также написана отдельная подпрограмма, принимающая в качестве входных параметров угол поворота и скорость поворота. Положительному углу поворота соответствует поворот робота вправо, а отрицательному – влево. При вызове функции текущий угол отклонения обнуляется, затем в цикле осуществляется поворот до тех пор, пока текущий угол не равен заданному значению угла. Поворот осуществляется вращением двигателей в противоположные стороны. Код функции поворота представлен ниже


void Rotate(float angle = 180, int speed=30){
	ResetMotor(motorB);
	ResetMotor(motorC);
	speed=abs(speed);
	if(angle>0) speed*=-1;
	angle = abs(angle);
	gyroAngle = 0;
	nSyncedMotors = synchBC;
	nSyncedTurnRatio = -100;
	motor[motorB] = speed;
	while (abs(gyroAngle) < angle){}
	ResetMotor(motorB);
	ResetMotor(motorC);
}


Для движения робота по прямой (вперед/назад) также написана отдельная функция, входными параметрами которой являются скорость движения и расстояние в сантиметрах, которое необходимо проехать. Код функции приведен ниже


void Move(int dist,int speed=40){
	if (dist*speed<0) speed = -abs(speed);
	dist = abs(dist);
	ResetMotor(motorB);
	ResetMotor(motorC);
	nSyncedMotors = synchBC ;
	nSyncedTurnRatio = 100;
	motor[motorB] = speed;
	while(abs(nMotorEncoder[motorB])*kMove<dist);
	ResetMotor(motorB);
	ResetMotor(motorC);
}


Теперь подробнее об алгоритмах, реализованных на базе гироскопа. В прошлой части я выложил ссылки на видео (в этой статье они продублированы), а теперь поподробнее как это работает.

Поиск ближайшего объекта робот начинает с поворота влево на 90°. Далее запускается цикл поиска объекта. Робот производит сканирование окружающего пространства ультразвуковым дальномером, полученное расстояние сравнивается с хранящимся в памяти минимальным расстоянием (на первой итерации оно равно 255). Если измеренное значение расстояния меньше минимального, то значение, соответствующее минимальному измеренному расстоянию обновляется, вместе с ним также обновляется и угол отклонения, при котором были получено данное минимальное значение расстояния. Цикл завершается при граничном угле в 90°. Таким образом робот сканируют переднюю полусферу и получает пару значений угол/расстояние, которые соответствуют ближайшему к роботу объекту. Затем производится поворот на полученный угол, движением до объекта на полученное расстояние (с корректировкой на размеры манипулятора) и последующий захват объекта.

Движение по траектории с возвратом в исходную точку работает следующим образом. В цикле от 1 до 10 (количество точек траектории) с помощью генератора случайных чисел задаются новые значения угла поворота и расстояния до новой точки, затем робот перемещается в новую точку. Одновременно с этим, в цикле производится вычисление суммарного угла отклонения, а также значения проекций по осям OX и OY. Проекции вычисляются на основе суммарного угла поворота. После выполнения цикла прохождения траектории, вычисляется угол, на который необходимо повернуться роботу, чтобы начать движение к исходной точке по кратчайшему маршруту. Затем вычисляется расстояние по формуле модуля длины вектора, и робот движется к исходной точке. После прихода в исходную точку, робот вычисляет итоговое отклонение угла от начального значения при запуске и поворачивает на вычисленное значение угла. Программа завершается.

В третьей части будет рассказано об использовании встроенного в контроллеры NXT беспроводного bluetooth-модуля.
  • +1
  • 31 мая 2016, 21:26
  • Skyer

Комментарии (0)

RSS свернуть / развернуть

Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.