Рука-манипулятор для соревневаний. Серво без delay ? Глючит энкодер.
- Войдите на сайт для отправки комментариев
Ср, 09/11/2016 - 22:27
Здавствуйте форумчане.
Есть такие вот соревнования для подростков:
http://http://robolymp.ru/rules-and-regulations/manipulyatory/
С сыном собрали из робота-пешехода вот это:
Код:
#include <Wire.h>
#include <Multiservo.h>
Multiservo PLECHO,LOKOT,ZAHVAT;
//ДАТЧИК ЦВЕТА
int s0 = 8;
int s1 = 9;
int s2 = 12;
int s3 = 11;
int out = 10;
int red,green,blue=0;
int CVET;//хранилище кода цвета
//МОТОР LEGO
int IN1 = 5;// Input1 подключен к выводу 5
int IN2 = 6;// Input2 подключен к выводу 6
int ENA = 7;// ENA подключен к выводу 7-скорость
int NAPRAVLENIE;//0-вправо,1-влево
volatile unsigned int encoder = 300;//начальное значение шагов энкодера
int Zahvat=150;//180-сжатие,115-открыть
int del=1000;
volatile unsigned long lastmillis = 0;
void setup() {
Wire.begin();
pinMode (ENA, OUTPUT);
pinMode (IN1, OUTPUT);
pinMode (IN2, OUTPUT);
pinMode(s0, OUTPUT);
pinMode(s1, OUTPUT);
pinMode(s2, OUTPUT);
pinMode(s3, OUTPUT);
pinMode(out, INPUT);
digitalWrite(s0, HIGH);
digitalWrite(s1, HIGH);
Serial.begin(9600);
PLECHO.attach(0);
LOKOT.attach(1);
ZAHVAT.attach(4);
PLECHO.writeMicroseconds(600);
LOKOT.writeMicroseconds(1250);
ZAHVAT.write(120);
attachInterrupt(4,Probeg,CHANGE);//на 19-ом пине меги
}
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<< void loop >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
void loop() {
// все срабатывает одновременно ,а нужно последоветельно .
// delay убрал во всей программе тк не срабатывал энкодер.
PODNYAT();
MOTOR_RIGHT(600);
OPUSTIT();
ZAXVAT_VZYAT();
PODNYAT();
}
void Probeg() //поворот робота(подразумевается,что этим смогу определить положение)
{
if( NAPRAVLENIE == 0 ){//когда поворачивается вправо
encoder = encoder+1;
}
else{
encoder = encoder-1;
}
}
void MOTOR_RIGHT(int d)// значение,на сколько повернется робот
{
if(encoder < d) {
digitalWrite (IN1, HIGH);
digitalWrite (IN2, LOW);
analogWrite(ENA,160);
NAPRAVLENIE=0;
}
else if(encoder > d) {
digitalWrite (IN1, LOW);
digitalWrite (IN2, HIGH);
analogWrite(ENA,160);
NAPRAVLENIE=1;
}
else if(encoder = d){
digitalWrite (IN1, LOW);
digitalWrite (IN2, LOW);
}
}
//<<<<<< ВЗЯТЬ КУБИК >>>>>>>>>>
void ZAXVAT_VZYAT(){
for (int i=120;i<=150;++i){
ZAHVAT.write(i);
//delay(20);
}}
//<<<<<< ОПУСТИТЬ КУБИК >>>>>>>>>>
void ZAXVAT_OTPUSTIT(){
for (int i=150;i>=120;--i){
ZAHVAT.write(i);
// delay(20);
}}
//<<<<<< ПОДНЯТЬ ЛОКОТЬ >>>>>>>>>>
void PODNYAT(){
for (int i=1250;i>=900;--i){
LOKOT.write(i);
// delay(20);
}}
//<<<<<< ОПУСТИТЬ ЛОКОТЬ >>>>>>>>>>
void OPUSTIT(){
for (int i=900;i<=1250;++i){
LOKOT.write(i);
// delay(20);
}}
Прерывания и серво вместе корректно не работают. По отдельности ( тест мотора и тест серво ) работают правильно. Мотор с энкодером встает на правильные градусы. Сервы то же .А в одной программе работать не хотят. Убрал все delay . Все команды начинают работать одновременно. А нужно чтобы плавно и последовательно .
Может кто то делал подобную программу?
Подскажите пожалуйста.
Если добавлять detachInterrupt, то вообще вырубается энкодер и мотор крутит бесконечно
Скажу честно, всю программу перечитывать не буду, тем более что не оригинал. Ошибка здесь скорее всего заключается в системном подходе.
И так есть функции которые вызываются.
Функции которые отвечаю за определённые суставы, допустим у нас есть плечо и локоть (ну там кисть и т.д. сейчас не суть).
Что делаете вы, вы говорите "Согни плечо до 90 град, согни локоть на 70 град, согни кисть на 30 град", а затем думаете что он согнёт сначала плечо, потом локоть, потом кисть - и видимо логично выходит "фиг вам". А именно - сгибается всё и сразу - и именно так всё и должно быть.
Вам в логике надо прописывать. Согни плечо на 90 град, когда он будет согнут - сгибай локоть до 70 град....
Вы в программе по сути задаёте координату к которой надо прийти - устанавливая углы.
Хотите - определённую траекторию движения, сначала это, потом то, потом вон то вон.
Если у вас нет обратной связи с моторов (а её скорее всего нет), то вы не можете знать, достиг мотор заданного угла или нет. Тогда вам на помощь приходит либо делей, либо флаг. Делей - это зло, лучше флаг.
Делайте проверку условий и вводити их. К примеру знаете что надоизменить угол с 90 на 60, и знаете что скорость вращения 1 градус в секунду для двигателя. Значит, 90-60 = 30 , задержку надо в 90 сек (конечно в реальности в разы меньше). Следовательно прописываете условия, "прочесть текущее значени, если текущее значение и требуемое различны, тогда: поднять флаг, прочесть текущее время, запомнить время, текущий угол - требуемый угол = модуль числа, модуль числа, получили градусы премещения, зная градус и количество градусов в секунду - вычислили сколько секунд будет идти движение, в счётчик добавляем отсечку по времени. Дальше у нас идёт цикл по условиям, если текущее время < время старта + время перемещения - нифига не делаем (крутимся в петле, по сути это тот же делей, но тут вопрос оптимизации вы можеет выполнять другие действия или не выполнять пока флаг не опустится), как только текущее время > время старта+время перемещения, опустить флаг - сделать следующее действие - тут уже действует следующий сустав.
Надеюсь понятно.
В такой логике суставы будут работать последовательно, поскольку у каждого в петле будет крутиться. Так же будет приоритет, т.е. определённая последовательность в изменении координаты. К примеру если первым идёт плечо - в нём локоть - в нём кисть, и вы скажите координаты, сначала двинется плечо, когда встанет на место, потом только локоть, потом только кисть.
Если захотите изменить кисть, надо будет задать нулевые изнения суставов с высшим приоритетом.
Надеюсь хоть что-то было понятно и полезно.
Большое спасибо за Ваш коментарий.
Прочитал, и понял что моих знаний явно не достаточно для решения этой задачи.
А что значит ''поднять флаг, опустить флаг" ?
Подскажите пожалуйста.
Как плавно вращать серву и задоно определять ее положение посмотрите здесь https://www.youtube.com/watch?v=yVxjzu__ukI&t=581s
Спасибо. Очень хорошее видео.
Евгений79, почитайте за "автоматное программирование". Это то, что Вам пытался разъяснить Phoenix.
Каждый сустав может представлять из себя такой конечный автомат: "останов (в положении..)", "движение (к положению)", "завершено (движение)". Процедура такого автомата представляет из себя обычный switch(){} или набор if(){}else{} блоков действий от заданного состояния:
Если состояние "останов", то проверяем есть ли команда и выполняем её. Команда запускает мотор или чего ещё, включает условие (например время завершения или условие по энкодеру) и изменяет состояние автомата на "движение".
Если состояние "движение", то проверяем достижение условия (или каких-то катастроф. случаев - аварий -> переход в состояние "ошибка движения") и пока оно не выполнено - возвращаем управление. Как только условие сработало, если надо то останавливаем мотор и изменяем свое состояние на "завершено".
Состояние завершено может выставлять флаги или вызывать внешние (callback) функции для уведомления остальной части программы что действие завершено и можно делать что-то ещё и переходим к состоянию "останов".
Управляющая программа может быть реализована точно также в виде своего конечного автомата. Надо только правильно расписать его состояния и переходы между ними. Весь loop() при автоматном программировании сводится к поочередному вызову набора конечных автоматов.
Суть автоматного программирования - разделение ситуаций на состояния и введение перемнной "текущее состояние автомата", которая используется в едином блоке ветвтления по состояниям автомата. И второй момент - конечный автомат - фиговина, которая сама себя переводит из одного состояния в другое или это делается извне командами управления автоматом. Собственно и фсё. :)
Пример реализации такого подхода есть в моей библиотеке тут: https://github.com/dimanus/ARHAT_H смотрите файлы tsc.h, tsc.c это как раз подход по унификации для автоматного программирования. Там же есть вариант реализации работы с прерываниями и управления по энкодерам или таймингам, в частности съем показаний с узв. датчика HCSR-04 по прерываниям (без ожидания эхо как в pulseIn()) - hcsr04.h, pcint.h, но там реализация достаточно мутная - попытка имитировать темплейты из С++ средствами С, зато есть примеры.
В целом, автоматное программирование - это то, что "доктор прописал" для задач управления. И, по большому счету, управляющий микроконтроллер - это не "AVR", "ARM" или что-то ещё. Такой подход должен быть зашит непосредственно в систему команд. Вот тогда - это действительно управляющий камень. А так .. баловство это всё.