Программирование роботов, осень 2014: Робот-манипулятор.

Манипулятор.Ханойские башни

Сергей Александрович Филиппов Санкт-Петербургский Физико-математический лицей №239

План занятия

Что такое манипулятор? Точное управление положением

двигателя. П-регулятор. Захват Два двигателя одновременно Три двигателя одновременно Устойчивость манипулятора Калибровка положений Управление скоростью движения

Манипулятор

это управляемое устройство, предназначенное для выполнения сложных действий, аналогичных движениям руки человека,

это механизм для управления положением предметов,

применяется при работе в трудных, опасных условиях,

применяется для замены человека при выполнении однообразных повторяющихся операций.

Управление положением двигателя Пропорциональный

регулятор: Уставка alpha Текущее положение e1 Невязка err=alpha-e1 Усиливающий

коэффициент k Управляющее

воздействие k*(alpha-e1) Из параллельной задачи

изменяется уставка alpha

Задание 1. Добавьте изменение положения в отрицательную сторону

Задание 2*. Замените время на ожидание нажатия датчика касания

Управление положением двигателя Пропорциональный

регулятор: Уставка alpha Текущее положение en1 –

показания энкодера Невязка e=alpha-en1 Усиливающий

коэффициент k Управляющее

воздействие u=k*e Из основной задачи

изменяется уставка alpha

Задание 1. Добавьте изменение положения в отрицательную сторону

Задание 2*. Замените время на ожидание нажатия датчика касания

int alpha=0, e, k=2, u;task motor1(){ while(true) { e=alpha-nMotorEncoder[motorA]; u=e*k; motor[motorA]=u; wait1Msec(1); }}task main(){ nMotorEncoder[motorA]=0; StartTask(motor1); while(true) { wait1Msec(2000); alpha=45; wait1Msec(2000); alpha=0; }}

Интегральный регулятор

Интегральный регулятор: Накапливает ошибки i=i+k*e*t Физический смысл —

длительность ошибки Постоянная t убирается в

коэффициент ki=k*t Для наблюдения за

работой интегральной составляющей обнуляется пропорциональный коэффициент kp

Накопление ошибки приводит к перерегулированию

int alpha=0;float i=0, kp=0, ki=0.01, p, e;task motor1(){ while(true) { e=alpha-nMotorEncoder[motorA]; p=kp*e; i=i+ki*e; u=p+i; motor[motorA]=u; wait1Msec(1); }}

Интегральный регулятор с ограничением Интегральный регулятор:

Накапливает ошибки i=i+k*e*t Физический смысл —

длительность ошибки Постоянная t убирается в

коэффициент ki=k*t Для наблюдения за

работой интегральной составляющей обнуляется пропорциональный коэффициент kp

Ограничение интегральной составляющей действует на небольших отклонениях

int alpha=0;float i=0, kp=0, ki=0.01, p, e;task motor1(){ while(true) { e=alpha-nMotorEncoder[motorA]; p=kp*e; if (abs(i)>maxi) i=sgn(i)*maxi; i=i+ki*e; u=p+i; motor[motorA]=u; wait1Msec(1); }}

Захват

Постройте захват на основе второго мотора

Примените к нему аналогичный алгоритм

На концах изогнутых балок зафиксируйте небольшие резиновые детали (колесики)

Два двигателя одновременно Установите захват на первый

двигатель При необходимости

используйте балку, удлиняющую плечо

Примените алгоритм управления двумя двигателями

Перед запуском установите захват в раскрытое положение

Подготовьте стаканчик для перемещения

Задание 1. Добавьте команды возврата и раскрытия захвата

Задание 2. Составьте алгоритм управления манипулятором двумя датчиками касания

Задание 3. Составьте алгоритм с использованием одного датчика расстояния: как только появляется новый стаканчик, робот хватает и перемещает его

Два двигателя одновременно Установите захват на первый

двигатель При необходимости

используйте балку, удлиняющую плечо

Примените алгоритм управления двумя двигателями

Перед запуском установите захват в раскрытое положение

Подготовьте стаканчик для перемещения

Задание 1. Добавьте команды возврата и раскрытия захвата

Задание 2. Составьте алгоритм управления манипулятором двумя датчиками касания

Задание 3. Составьте алгоритм с использованием одного датчика расстояния: как только появляется новый стаканчик, робот хватает и перемещает его

int alpha=0, beta=0;task motor1(){ while(true) { int e1=nMotorEncoder[motorA]; int e2=nMotorEncoder[motorB]; motor[motorA]=2*(alpha-e1); motor[motorB]=2*(beta-e2); wait1Msec(1); }}task main(){ nMotorEncoder[motorA]=0; nMotorEncoder[motorB]=0; StartTask(motor1); while(true) { wait1Msec(2000); alpha=90; wait1Msec(2000); beta=90; }}

Ожидание появления объектаwhile(true) {

wait1Msec(2000);

alpha=90; // направо

wait1Msec(2000);

while(SensorValue[S1]>15)

wait1Msec(10); // объект

wait1Msec(2000);

beta=90; // захват

...

}

Ожидание появления объекта перед захватом манипулятора с помощью датчика расстояния

Плавное перемещение с виртуальной целью В качестве промежуточной уставки вводится переменная (a и b),

значение которой равномерно изменяется в сторону главной уставки (alpha и beta)a=a+sgn(alpha-a)*dab=b+sgn(beta-b)*db

Коэффициенты da и db и задержка (10 мс) регулируют скорость моторов (по умолчанию 100º/с) task motor1(){ int a=0, b=0, ka=1, kb=1, da=1, db=1; while(true) { int e1=nMotorEncoder[motorA]; int e2=nMotorEncoder[motorB]; a=a+sgn(alpha-a)*da; b=b+sgn(beta-b)*db; motor[motorA]=ka*(a-e1); motor[motorB]=kb*(b-e2); wait1Msec(10); }}

Управление тремя моторами Снимите захват Закрепите третий мотор

как показано на рисунке Укрепите конструкцию

лучевыми балками Переключите моторы в

порядке следования Примените алгоритм для

управления тремя моторами

Перед запуском установите моторы в исходное положение

Управление тремя моторами Задайте команды для перемещения стаканчика: вверх,

вправо, вниз, отпустить. На лучевых балках установите пирамидки для надевания

стаканчиков на них При необходимости отрегулируйте усиливающие

коэффициенты

Система обезвешивания Второй (центральный) мотор испытывает максимальную

нагрузку при подъеме Система обезвешивания (резинка) уравновешивает

нагрузку на подъем и опускание

Калибровка положений

После инициализации энкодеров вводится блок калибровки

Робот просит установить мотор в соответствующее положение и нажать кнопку

Положение запоминается в переменную

bFloatDuringInactiveMotorPWM=true;

// Левое положение

NxtDisplayBigTextLine(1,”A1-left”);

while(nNxtButtonPressed!=3);

while(nNxtButtonPressed==3);

A1 = nMotorEncoder[motorA];

PlaySound(soundBeepBeep);

// Центральное положение

NxtDisplayBigTextLine(1,”A2-center”);

...

Манипулятор с редуктором Усиление второго мотора с

помощью механической передачи

Требуется рычажок для позиционирования мотора

В программе необходимо установить реверс мотора

Захват с подъемником

Две степени свободы в одном механизме

Замыкание передачи на подъем при захвате объекта

Контроль давления: сила захвата зависит от веса объекта

Захват с подъемником - 2

Благодарю за внимание!

Сергей Александрович Филиппов Физико-Математический лицей № 239

Санкт-Петербург[email protected]