Рука-манипулятор для соревневаний. Серво без delay ? Глючит энкодер.

Евгений 79
Евгений 79 аватар
Offline
Зарегистрирован: 15.06.2014

Здавствуйте форумчане.

Есть такие вот соревнования для подростков:

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, то вообще вырубается энкодер и мотор крутит бесконечно

Phoenix
Phoenix аватар
Offline
Зарегистрирован: 02.11.2016

Скажу честно, всю программу перечитывать не буду, тем более что не оригинал. Ошибка здесь скорее всего заключается в системном подходе.

И так есть функции которые вызываются. 
Функции которые отвечаю за определённые суставы, допустим у нас есть плечо и локоть (ну там кисть и т.д. сейчас не суть).

Что делаете вы, вы говорите "Согни плечо до 90 град, согни локоть на 70 град, согни кисть на 30 град", а затем думаете что он согнёт сначала плечо, потом локоть, потом кисть - и видимо логично выходит "фиг вам". А именно - сгибается всё и сразу - и именно так всё и должно быть.

Вам в логике надо прописывать. Согни плечо на 90 град, когда он будет согнут - сгибай локоть до 70 град....

Вы в программе по сути задаёте координату к которой надо прийти - устанавливая углы.
Хотите - определённую траекторию движения, сначала это, потом то, потом вон то вон.

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

Делайте проверку условий и вводити их. К примеру знаете что надоизменить угол с 90 на 60, и знаете что скорость вращения 1 градус в секунду для двигателя. Значит, 90-60 = 30 , задержку надо в 90 сек (конечно в реальности в разы меньше). Следовательно прописываете условия, "прочесть текущее значени, если текущее значение и требуемое различны, тогда: поднять флаг, прочесть текущее время, запомнить время, текущий угол - требуемый угол = модуль числа,  модуль числа, получили градусы премещения, зная градус и количество градусов в секунду - вычислили сколько секунд будет идти движение, в счётчик добавляем отсечку по времени. Дальше у нас идёт цикл по условиям, если текущее время < время старта + время перемещения - нифига не делаем (крутимся в петле, по сути это тот же делей, но тут вопрос оптимизации вы можеет выполнять другие действия или не выполнять пока флаг не опустится), как только текущее время > время старта+время перемещения, опустить флаг - сделать следующее действие - тут уже действует следующий сустав.

Надеюсь понятно.
В такой логике суставы будут работать последовательно, поскольку у каждого в петле будет крутиться. Так же будет приоритет, т.е. определённая последовательность в изменении координаты. К примеру если первым идёт плечо - в нём локоть - в нём кисть, и вы скажите координаты, сначала двинется плечо, когда встанет на место, потом только локоть, потом только кисть. 
Если захотите изменить кисть, надо будет задать нулевые изнения суставов с высшим приоритетом. 

Надеюсь хоть что-то было понятно и полезно.

Евгений 79
Евгений 79 аватар
Offline
Зарегистрирован: 15.06.2014

Большое спасибо за Ваш коментарий.
Прочитал, и понял что моих знаний явно не достаточно для решения этой задачи.

А что значит ''поднять флаг, опустить флаг" ?
Подскажите пожалуйста.

JasKo
Offline
Зарегистрирован: 21.11.2015

Как плавно вращать серву и задоно определять ее положение посмотрите здесь https://www.youtube.com/watch?v=yVxjzu__ukI&t=581s

Евгений 79
Евгений 79 аватар
Offline
Зарегистрирован: 15.06.2014

Спасибо. Очень хорошее видео.

Arhat109-2
Offline
Зарегистрирован: 24.09.2015

Евгений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" или что-то ещё. Такой подход должен быть зашит непосредственно в систему команд. Вот тогда - это действительно управляющий камень. А так .. баловство это всё.