roboforum.ru

#include <Servo.h>
#define kolvoKAD 20//количество кадров-тактов
#define kolvoSV 2//количество сервоприводов (1-6 штук)
//сервоприводы подключаются Arduino UNO: сигнальный провод - со 2 вывода до 7, обратной связи с 14 по 19 (А0-А5)
Servo servo[kolvoSV];

byte ugolServ [kolvoKAD][kolvoSV];//массив-таблица хранения значений углов серв по кадрам
int tabl_da[kolvoSV];//массив переменных приращения углов серв
int tabl_ga[kolvoSV];//массив переменных длительностей между переключениями серв
long tabl_Ya[kolvoSV];//массив переменных хранения моментов времени
int tabl_poprFor[kolvoSV];//массив хранения поправок к формуле для каждой сервы

int n=500;//длительность общая шага-цикла в мс
int i=0;//счётчик элементов массивов
int j=0;//счётчик элементов массивов

boolean fl_A=false;//флаг состояний
boolean fl_B=false;//флаг состояний
boolean fl_C=false;//флаг состояний

void setup() {
   Serial.begin(9600);
   pinMode(13,OUTPUT);//вывод канала индикации
   digitalWrite(13, LOW);
   pinMode(10,INPUT);//вывод кнопки записи-воспроизведения движений серв
  for(i=0;i<kolvoSV;i++){servo[i].attach(i+2);servo[i].write(90);delay(500); servo[i].detach();}//начальные установки углов 90 градусов (подкл. на выводы начиная со 2 по возр.) 

///////////////// вносим данные поправок для каждой сервы ///////////////
tabl_poprFor[0]=0;
tabl_poprFor[1]=-4;
/////////////////////////////////////////////////////////////////////////
}
void loop() {
if(digitalRead(10)==LOW) {delay(300);//временная пауза для преодоления дребезга контактов кнопки

if(digitalRead(10)==HIGH){//если происходит краткое нажатие кнопки
zapis();}//функция на запись выставленных положений качалок для одного кадра

if(digitalRead(10)==LOW){//если происходит долгое нажатие кнопки
ispol_();}//функция на исполнение записанных в массив кадров
}
}
void ispol_()
{
  digitalWrite(13, HIGH);//включаем светодиод постоянно - пошло исполнение записанного
  for(i=0;i<kolvoSV;i++){servo[i].attach(i+2);servo[i].write((analogRead(14+i) - 75+tabl_poprFor[i]) / 3);}
 
  while(true)//запускается бесконечный цикл ходьбы
  {
     for (j=0;j<kolvoKAD;j++)//проверяем все углы по кадрам-тактам
   
   {
      if( fl_C==false) {Serial.println(" ");}//печатаем "пустую строку" между каждым кадром-тактом
       
       for(i=0;i<kolvoSV;i++){if(ugolServ [j][i]!=0){fl_A=true;break;}}
       if(fl_A==true)//если хоть один элемент кадра-такта не 0
     
     {
      fl_A=false;//взведение флага
      ///////////////////
       for(i=0;i<kolvoSV;i++)
       {
        if( fl_C==false){Serial.print("КАДР ");Serial.print(j);Serial.print(" СЕРВОПРИВОД ");
        Serial.print(i);Serial.print(" УГОЛ ");Serial.println(ugolServ [j][i]);}//печать в монитор выставленных углов по приводам и кадрам при первом проходе массива
       
        if(ugolServ [j][i]-servo[i].read()>0){tabl_da[i]=1;}//определяем есть ли приращение и его знак
        if(ugolServ [j][i]-servo[i].read()<0){tabl_da[i]=-1;}
        if(ugolServ [j][i]-servo[i].read()==0){tabl_da[i]=0;}
        if(tabl_da[i]!=0){tabl_ga[i]=n/abs(ugolServ [j][i]-servo[i].read());}//определяем временную паузу между приращениями шага
        if(tabl_da[i]==0){tabl_ga[i]=0;}
        }
      /////////////////
      do
       {
        fl_B=true;
            for (i=0; i<kolvoSV ; i++) {
              if (ugolServ [j][i]!=servo[i].read()) {
               fl_B=false;
                break;
                  }
                  }
         for(i=0;i<kolvoSV;i++)
         {
         if(millis()- tabl_Ya[i]>tabl_ga[i]&&ugolServ [j][i]!=servo[i].read()){tabl_Ya[i]=millis(); servo[i].write(servo[i].read()+tabl_da[i]);}
     
         }
      }while(!fl_B); // пока все сервы не придут к табличному углу поворота...
     }
   }
    fl_C=true;//сигнал на прерывание повторной печати в монитор порта
  }
}
/////////////////////////////////////////////////////////////

void zapis()
{
for(i=0;i<kolvoSV;i++){ugolServ [j][i]=(analogRead(14+i) - 75+tabl_poprFor[i]) / 3;}j++;if(j>=kolvoKAD){j=0;}//запись выставленных значений углов в массив
  digitalWrite(13, HIGH);delay(300);digitalWrite(13, LOW);//кратко мигаем светодиодом -запись прошла 
}