Бывает такое , пишешь сообщение на форум , включаешь в него код и коротко описываешь алгоритм, кажется что все понятно читателю и нет необходимости подробно описывать алгоритм (идею) кода
Через некоторое время читаешь свое сообщение и ничего не понимаешь. Вот такая вот польза от чтения своих прошлых сообщений. Приходится переписывать сообщение и более подробно описывать алгоритм кода
Сейчас пишу программу для вертолета чтобы он летал по квадрату и решил воспользоваться кодом для поворота на фиксированный угол
Алгоритм такой
Постановка: Допустим нужно повернуться справа-налево (против часовой стрелки) на определенный угол.
Решение: Сначала измеряем текущий (начальный) угол, потом вычисляем конечный, то есть прибавляем к начальному углу значение угла на который требуется повернуть вертолет. Начинаем поворот, в цикле все время проверяем условие, что текущий угол меньше конечного. Если условие истинно то продолжаем поворот, если ложно то выходим из цикла
Такой способ работает если в процессе поворота не встретилась особая точка в которой угол поворота меняется скачком от плюс 180 до минус 180 градусов.
Что же делать если это случилось?
Допустим начальный угол равен 100 градусам, а повернуться надо на угол 90 градусов. Следуя нашему алгоритму складываем 100 и 90, получаем 190
В "особой точке" происходит переход от положительных значений к отрицательным, то есть будет выполнено условие, что в двух последовательных измерениях угла поворота, предыдущий угол будет больше чем последующий.
Хотя это условие корректно с логической точки зрения при вращательном движении, в реальности, перед началом вращения, когда вертолет еще не среагировал на команду move3D(vx, vy, vz, vr), он будет шарахаться в разные стороны от своего начального направления в режиме hover (зависания в точке) и поэтому может случится, что условие будет выполнено в самом начале поворота и произойдет выход из цикла. Визуально это выглядит как микродерганье вертолета
Поэтому я использовал другое для установки флажка, который устанавливается в true при проходе "особой точки"
if (angle_current < 0 && angle_prev > 170) {
angle_current = angle_current + 360;
flag = true;
}Это ключевое условие которое позволяет нам зафиксировать факт наличия "особой точки" в процессе измерения углов поворота и путем прибавления к измеренному отрицательному углу числа 360, получить положительное значение угла которое будет больше чем предыдущее значение
Зафиксировать наличие "особой точки" в программе можно с помощью логической переменной, которая будет менять свое значение если точка пройдена в процессе поворота. И далее для получения значения угла поворота к измеренному отрицательному значению будем прибавлять число 360
Сама процедура поворота на заданный угол выглядит следующим образом
// Поворот на заданный угол
void ARDrone::loopCommand5(void)
{
double angle_start, angle_finish, angle, angle_current, angle_prev;
angle = 90; // угол поворота
angle_start = getYaw()*RAD_TO_DEG; // начальный угол
angle_prev = angle_start;
angle_finish = angle_start + angle - 10; // конечный угол
double vx = 0.0, vy = 0.0, vz = 0.0, vr = 1.0;
boolean flag = false;
while (1) {
move3D(vx, vy, vz, vr);
angle_current = getYaw()*RAD_TO_DEG; // текущий угол
// если "особая точка" пройдена, то прибавляем к измеренному значению число 360
if (flag) angle_current = angle_current + 360;
// Если текущее и предыдущее значение углов не равны
if (angle_current != angle_prev) {
if (angle_current < 0 && angle_prev > 170) {
angle_current = angle_current + 360;
flag = true;
}
if (angle_current >= angle_finish) break;
}
angle_prev = angle_current;
msleep(1);
}
vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0;
move3D(vx, vy, vz, vr);
}Еще нужно сделать такое замечание. Хотя повернуть в данном случае вертолет нужно на 90 градусов, формула для расчета конечного угла выглядит так
angle_finish = angle_start + angle - 10; // конечный угол
Видно что из угла вычитается 10 градусов. Это связано с инерцией срабатывания двигателей вертолета. То есть команда остановки выполняется не сразу, с запозданием. Поэтому нужно выдавать с упреждением
Войти
Регистрация
Наверх
