roboforum.ru

//ПАУК (ГЕКСАПОД) 3 СЕРВОПРИВОДА ТЕСТ ОБУЧЕНИЕ ПОЛЬЗОВАТЕЛЯ
//
#include <Servo.h>
byte dd=10;//амплитуда ходьбы
byte n=5;// постоянная временного интервала
byte i=0;//счётчик
byte j=0;//счётчик
byte a=90;//угол "серединный"
byte b=90;//угол "серединный"
byte c=95;//угол "серединный"
long Y=0;//переменная хранения моментов времени
String inputString;// строки данных

/////////////////////////////
long Y1=0;//переменная хранения моментов времени
long Y2=0;//переменная хранения моментов времени
long Y3=0;//переменная хранения моментов времени
int POR_1=10;//значение порога переключения 1 группы
int POR_2=10;//значение порога переключения 2 группы
int POR_3=10;//значение порога переключения 3 группы
////////////////////////////
byte ves_1=3;//значение веса "зелёный" (значения от 1 до 3 включительно)
byte ves_2=2;//значение веса  "жёлтый" 
byte ves_3=1;//значение веса  "красный"
////////////////////////////
byte fl_1=1;//флаг состояний 1 группы
byte fl_2=1;//флаг состояний 2 группы
byte fl_3=1;//флаг состояний 3 группы
byte srab_1=0;//переменные счёта числа срабатываний группы кнопок 1
byte srab_2=0;//переменные счёта числа срабатываний группы кнопок 2
byte srab_3=0;//переменные счёта числа срабатываний группы кнопок 3
////////////////////////////
Servo servo_a;
Servo servo_b;
Servo servo_c;

void setup()
{
 
  Serial.begin(9600);
  servo_a.attach(5);//вывод управления сервоприводом ПРАВАЯ ПАРА
  servo_a.write(a+dd);// установка начального значения
  
  servo_b.attach(6);//вывод управления сервоприводом ЛЕВАЯ ПАРА
  servo_b.write(b+dd);// установка начального значения
  
  servo_c.attach(7);//вывод управления сервоприводом ЦЕНТРАЛЬНАЯ ПАРА
  servo_c.write(c-dd);// установка начального значения
 }
/////////////////////////////////////////////////////////////////////////////////////////
void loop()
{
  CheckSerial();//ЖДЁМ ПОСТУПЛЕНИЯ КОМАНДЫ
  //fl_1=1;fl_2=1;fl_3=1;//режим обычного пульта
}
/////////////////////////////////////////////////////////////////////////////////////////
void vpered(byte k){
   for(j=0;j<k;j++){
  ///////////// ВПЕРЁД  /////////////
  servo_c.write(c-dd);// установка начального значения
  servo_a.write(a+dd);// установка начального значения
  servo_b.write(b+dd);// установка начального значения
  
  /////////// такт 1 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_a.write(servo_a.read()-1);
  servo_b.write(servo_b.read()-1);
  delay_(n);
  }
  /////////// такт 2 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_c.write(servo_c.read()+1);
  delay_(n);
  }
 /////////// такт 3 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_a.write(servo_a.read()+1);
  servo_b.write(servo_b.read()+1);
  delay_(n);
  } 
   /////////// такт 4 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_c.write(servo_c.read()-1);
  delay_(n);
  } 
}

}
///////////////////////////////////////////////////////////////////////////////////////
void nazad(byte k){
   for(j=0;j<k;j++){
  ///////////// НАЗАД  /////////////
  servo_c.write(c+dd);// установка начального значения
  servo_a.write(a+dd);// установка начального значения
  servo_b.write(b+dd);// установка начального значения
  
  /////////// такт 1 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_a.write(servo_a.read()-1);
  servo_b.write(servo_b.read()-1);
  delay_(n);
  }
  /////////// такт 2 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_c.write(servo_c.read()-1);
  delay_(n);
  }
 /////////// такт 3 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_a.write(servo_a.read()+1);
  servo_b.write(servo_b.read()+1);
  delay_(n);
  } 
  /////////// такт 4 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_c.write(servo_c.read()+1);
  delay_(n);
  }  
}
 
}
/////////////////////////////////////////////////////////////////////////////////////////
void levo(byte k){
   for(j=0;j<k;j++){
  ///////////// ВЛЕВО /////////////
  servo_c.write(c-dd);// установка начального значения
  servo_a.write(a+dd);// установка начального значения
  servo_b.write(b-dd);// установка начального значения
  
  /////////// такт 1 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_a.write(servo_a.read()-1);
  servo_b.write(servo_b.read()+1);
  delay_(n);
  }
  /////////// такт 2 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_c.write(servo_c.read()+1);
  delay_(n);
  }
 /////////// такт 3 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_a.write(servo_a.read()+1);
  servo_b.write(servo_b.read()-1);
  delay_(n);
  } 
   /////////// такт 4 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_c.write(servo_c.read()-1);
  delay_(n);
  } 
}
 
}
///////////////////////////////////////////////////////////////////////////////////////////
void pravo(byte k){
   for(j=0;j<k;j++){
  ///////////// ВПРАВО /////////////
  servo_c.write(c-dd);// установка начального значения
  servo_a.write(a-dd);// установка начального значения
  servo_b.write(b+dd);// установка начального значения
  
  /////////// такт 1 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_a.write(servo_a.read()+1);
  servo_b.write(servo_b.read()-1);
  delay_(n);
  }
  /////////// такт 2 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_c.write(servo_c.read()+1);
  delay_(n);
  }
 /////////// такт 3 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_a.write(servo_a.read()-1);
  servo_b.write(servo_b.read()+1);
  delay_(n);
  } 
   /////////// такт 4 ///////////
  for(i=0;i<2*dd;i++)
  {
  servo_c.write(servo_c.read()-1);
  delay_(n);
  } 
}

}
///////////////////////////////////////////////////////////////////////////////////////////
////  ФУНКЦИИ ОПИСАНИЕ // вместо задержки времени через delay  ////////////////////////////////////////
void delay_(int T1)
{
 Y=millis();
 while(millis()-Y<T1) 
  {
    // CheckSerial();//ЖДЁМ ПОСТУПЛЕНИЯ КОМАНДЫ
  }
}
 ////////////////////////////////////////////////////////////////////
 /////////////////////////////////////////////////////////////////////////////////////////////////
//ФУНКЦИИ ПРИЁМА И ОБРАБОТКИ ДАННЫХ
void CheckSerial()
{
  //////////////////////////////////////////////////////////////////////////////
  while (Serial.available())//считываем строку данных пока они поступают
  {
    char inChar = (char)Serial.read();
    if (inChar == '\n')     //если окончание строки
    {
     
      MakeCmd();//обрабатываем её данной функцией
      break;
    }
    else inputString += inChar;// иначе удлиняем строку на один символ
  }
}
/////////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
void MakeCmd()
{
  int y = inputString.length(); // присваиваем переменной у число символов в строке
  if (y < 1 || y > 1)return; //если их меньше 1 или больше, выходим из функции
  String cmd = inputString; // передаём строку другой переменной
  inputString = "";//"обнуляем переменную"
/////////// БЛОК ПРОГРАММНОЙ ОБРАБОТКИ (ВНУТРЕННИЙ СЛОЙ НЕЙРОСЕТИ) ////////////
 if(millis()-Y1>ves_3*5000){fl_1=1;srab_1=0;}
 if(millis()-Y2>ves_2*4000){fl_2=1;srab_2=0;}
 if(millis()-Y3>ves_1*4000){fl_3=1;srab_3=0;} 
 ////////// 
  if (cmd == "1") {
 Y1=millis();  
 if(fl_1==1){srab_1=srab_1+ves_1;if(srab_1>POR_1){srab_1=0;fl_1=2;}vpered(ves_3);}else{
 if(fl_1==2){srab_1=srab_1+ves_3;if(srab_1>POR_1){srab_1=0;fl_1=3;}nazad(ves_3);}else{
 if(fl_1==3){srab_1=srab_1+ves_2;if(srab_1>POR_1){srab_1=0;fl_1=1;}levo(ves_3);}
  }}}
  if (cmd == "2") {
   Y1=millis();  
 if(fl_1==1){srab_1=srab_1+ves_2;if(srab_1>POR_1){srab_1=0;fl_1=2;}levo(ves_3);}else{
 if(fl_1==2){srab_1=srab_1+ves_1;if(srab_1>POR_1){srab_1=0;fl_1=3;}vpered(ves_3);}else{
 if(fl_1==3){srab_1=srab_1+ves_3;if(srab_1>POR_1){srab_1=0;fl_1=1;}nazad(ves_3);}
  }}}
  if (cmd == "3") {
   Y1=millis();  
 if(fl_1==1){srab_1=srab_1+ves_3;if(srab_1>POR_1){srab_1=0;fl_1=2;}nazad(ves_3);}else{
 if(fl_1==2){srab_1=srab_1+ves_2;if(srab_1>POR_1){srab_1=0;fl_1=3;}levo(ves_3);}else{
 if(fl_1==3){srab_1=srab_1+ves_1;if(srab_1>POR_1){srab_1=0;fl_1=1;}vpered(ves_3);}
  }}} 
  
  /////////////
  if (cmd == "4") {
   Y2=millis();  
 if(fl_2==1){srab_2=srab_2+ves_1;if(srab_2>POR_2){srab_2=0;fl_2=2;}vpered(ves_2);}else{
 if(fl_2==2){srab_2=srab_2+ves_3;if(srab_2>POR_2){srab_2=0;fl_2=3;}nazad(ves_2);}else{
 if(fl_2==3){srab_2=srab_2+ves_2;if(srab_2>POR_2){srab_2=0;fl_2=1;}levo(ves_2);}
  }}} 
  if (cmd == "5") {
   Y2=millis();  
 if(fl_2==1){srab_2=srab_2+ves_2;if(srab_2>POR_2){srab_2=0;fl_2=2;}levo(ves_2);}else{
 if(fl_2==2){srab_2=srab_2+ves_1;if(srab_2>POR_2){srab_2=0;fl_2=3;}vpered(ves_2);}else{
 if(fl_2==3){srab_2=srab_2+ves_3;if(srab_2>POR_2){srab_2=0;fl_2=1;}nazad(ves_2);}
  }}}
  
  if (cmd == "6") {
 Y2=millis();  
 if(fl_2==1){srab_2=srab_2+ves_3;if(srab_2>POR_2){srab_2=0;fl_2=2;}nazad(ves_2);}else{
 if(fl_2==2){srab_2=srab_2+ves_2;if(srab_2>POR_2){srab_2=0;fl_2=3;}levo(ves_2);}else{
 if(fl_2==3){srab_2=srab_2+ves_1;if(srab_2>POR_2){srab_2=0;fl_2=1;}vpered(ves_2);}
  }}} 
  
  
  //////////////
  if (cmd == "7") {
  Y3=millis();  
 if(fl_3==1){srab_3=srab_3+ves_1;if(srab_3>POR_3){srab_3=0;fl_3=2;}vpered(ves_1);}else{
 if(fl_3==2){srab_3=srab_3+ves_3;if(srab_3>POR_3){srab_3=0;fl_3=3;}nazad(ves_1);}else{
 if(fl_3==3){srab_3=srab_3+ves_2;if(srab_3>POR_3){srab_3=0;fl_3=1;}levo(ves_1);}
  }}} 
  
  if (cmd == "8") {
  Y3=millis();  
 if(fl_3==1){srab_3=srab_3+ves_2;if(srab_3>POR_3){srab_3=0;fl_3=2;}levo(ves_1);}else{
 if(fl_3==2){srab_3=srab_3+ves_1;if(srab_3>POR_3){srab_3=0;fl_3=3;}vpered(ves_1);}else{
 if(fl_3==3){srab_3=srab_3+ves_3;if(srab_3>POR_3){srab_3=0;fl_3=1;}nazad(ves_1);}
  }}} 
  
  if (cmd == "9") {
 Y3=millis();  
 if(fl_3==1){srab_3=srab_3+ves_3;if(srab_3>POR_3){srab_3=0;fl_3=2;}nazad(ves_1);}else{
 if(fl_3==2){srab_3=srab_3+ves_2;if(srab_3>POR_3){srab_3=0;fl_3=3;}levo(ves_1);}else{
 if(fl_3==3){srab_3=srab_3+ves_1;if(srab_3>POR_3){srab_3=0;fl_3=1;}vpered(ves_1);}
  }}}   
  
///////////// 

}

///////////////////////////////////////////////////////////////////////////////////////////////////////