roboforum.ru

#include <Servo.h> //используем библиотеку

Servo myservo1;  //указываем сервопривод передних конечностей
Servo myservo2;  //указываем сервопривод задних конечностей
int a1=125; //граничные углы для сервы 1
int a2=55;
int b1=125;// граничные углы для сервы 2
int b2=55;
int a;int b;//переменные для хранения значений текущих углов
int n=20; //временная задержка
int n1=20; //коэффициент "плавности хода"
int c=0;//переменная числа шагов
int v=1;int v1=1;
long Y1=0;long Y2=0;
void setup()
{
  pinMode (5,INPUT );//подключение оптического датчика
  myservo1.attach(7);//выводы для подключения 1 и 2 сервопривода
  myservo2.attach(6);
  myservo1.write(a1);//начальная установка положения конечностей
  myservo2.write(b1);
   delay(n);
  myservo1.detach();myservo2.detach();// пауза для установки робота после включения
  delay(200*n);myservo1.attach(7);myservo2.attach(6);
}

void loop()
{
metka0:v=1;
  while(c<10)//условие числа шагов
  {
  //////////////////  ШАГ ВПЕРЁД или НАЗАД С РАЗВОРОТОМ ///////////////////////
  /////////////////////   ТАКТ 1  //////////////////////
  myservo1.write(a1);//"положение вала статичной сервы"
  delay_(n);if(v==0){goto metka0;}
  b=b1;
  while(b>=b2)
  {
   myservo2.write(b);//"положение вала динамичной сервы"
   delay_(n);if(v==0){goto metka0;} b=b-(b1-b2)/n1;
  }
  /////////////////////   ТАКТ 2  //////////////////////
  myservo2.write(b2);//"положение вала статичной сервы"
  delay_(n);if(v==0){goto metka0;}
  a=a1;
  while(a>=a2)
  {
   myservo1.write(a);//"положение вала динамичной сервы"
   delay_(n);if(v==0){goto metka0;} a=a-(a1-a2)/n1;
  }
  /////////////////////   ТАКТ 3  //////////////////////
  myservo1.write(a2);//"положение вала статичной сервы"
  delay_(n);if(v==0){goto metka0;}
  b=b2;
  while(b<=b1)
  {
   myservo2.write(b);//"положение вала динамичной сервы"
   delay_(n);if(v==0){goto metka0;} b=b+(b1-b2)/n1;
  }
  /////////////////////   ТАКТ 4  //////////////////////
  myservo2.write(b1);//"положение вала статичной сервы"
  delay_(n);if(v==0){goto metka0;}
  a=a2;
  while(a<=a1)
  {
   myservo1.write(a);//"положение вала динамичной сервы"
   delay_(n);if(v==0){goto metka0;} a=a+(a1-a2)/n1;
  }
  c++;}c=0;v1=1;
  //Условие смены направления движения и темпа после разворачивания с псевдослучайным движением прямолинейно или криволинейно
  myservo1.detach();myservo2.detach();delay(100*n);  myservo1.attach(7);  myservo2.attach(6);
  if (random(0,2)==1){a1=125;a2=55;b1=125;b2=55;}else{a1=113;a2=47;b1=133;b2=67;}n1=20;

 
                                         
}
////////////////////////////////////////   ФУНКЦИИ ОПИСАНИЕ   //////////////////////////////////////////////////////
//Функция заменяет выполнение команды delay(n); в ней на время паузы  идёт опрос датчика

void delay_(int T1)
{
Y2=millis();metka:Y1=millis();if(Y1-Y2>=T1){}
else {if(digitalRead(5)==LOW&&v1==1){myservo1.detach();myservo2.detach();delay(100*n);
myservo1.attach(6);  myservo2.attach(7);a1=113;a2=47;b1=133;b2=67;n1=10;c=0;v=0;v1=0;}goto metka;}

}

//////////////////////////    скетч для управления 4 сервоприводами бипода   /////////////////
#include <Servo.h> //используем библиотеку

Servo myservoLS3 ;  //указываем сервопривод ЛЕВОЙ СТОПЫ
Servo myservoLB4;  //указываем сервопривод ЛЕВОГО БЕДРА
Servo myservoRB5 ;  //указываем сервопривод ПРАВОГО БЕДРА
Servo myservoRS6;  //указываем сервопривод ПРАВОЙ СТОПЫ
int n=20;// временнАя задержка
int n1=3; //коэффициент "плавности хода"
int a03=87;int a04=95;int a05=90;int a06=77;//переменные для углов начального положения конечностей
int da=10;//переменная амплитуды изменения угла в градусах для стоп
int db=25;//переменная амплитуды изменения угла в градусах для  бедра
int f=0;// переменная - флаг
int a3=a03;int a4=a04;int a5=a05;int a6=a06;// переменные для хранения значений текущих углов
////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "boarddefs.h"   //Добавочная библиотека
#include "IRremote.h"
#include "IRremoteInt.h" //Добавочная библиотека

int RECV_PIN = 2;        //Пин подключения выходного сигнала с ИК-приёмника
int N=0;                 //Счётчик номера нажатой кнопки
long Y1=0;long Y2=0;
//Создаём экземпляр класса IRrecv, в качестве параметра передаём пин подключения сигнала ИК-приёмника
IRrecv irrecv(RECV_PIN); 

decode_results results;   //Переменная для сохранения полученного кода нажатой кнопки
/////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup()
{
 
myservoLS3.attach(3);//выводы  для прямохождения и подключения 1-4 сервоприводов
myservoLB4.attach(4);
myservoRB5.attach(5);
myservoRS6.attach(6);
myservoLS3.write(a03); delay(25*n);// начальная установка-настройка углов поворота валов серв
myservoLB4.write(a04); delay(25*n);myservoLB4.detach();
myservoRB5.write(a05); delay(25*n);myservoRB5.detach();
myservoRS6.write(a06); delay(25*n);
irrecv.enableIRIn();     //Включение ИК-приёмника в работу
}
void loop()
{
  ///////////////////////////////////////////
  delay_(10*n);
  //////////////////////////////////////  ХОЖДЕНИЕ ПРЯМО //////////////////////////////////////
  metka1:
  if(N==1){if(f==1){myservoLS3.attach(3);myservoLB4.attach(4);myservoRB5.attach(5); myservoRS6.attach(6); }f=0;
  ////////////////////////////////   ТАКТ НАКЛОН ВПРАВО ///////////////////////////////////////
  myservoRS6.attach(6);
while((a3<=a03+da)||(a6<=a06+da))
  { myservoLS3.write(a3);myservoRS6.write(a6);a3=a3+n1;a6=a6+n1; delay_(n);}
  myservoLS3.detach();delay_(n);
 
   //////////////////////////// ТАКТ ПОВОРОТ КОРПУСА ОТН. ПРАВОЙ НОГИ  //////////////////////////
  myservoLB4.attach(4);myservoRB5.attach(5);
while((a4>=a04-db)||(a5>=a05-db))
  { myservoLB4.write(a4);myservoRB5.write(a5);a4=a4-n1;a5=a5-n1; delay_(n);}
myservoLB4.detach();myservoRB5.detach();delay_(n);

 
/////////////////////////////////   ТАКТ НАКЛОН ВЛЕВО  //////////////////////////////////////
myservoLS3.attach(3);
while ((a3>=a03-da)||(a6>=a06-da))
  { myservoLS3.write(a3);myservoRS6.write(a6);a3=a3-n1;a6=a6-n1; delay_(n);}
  myservoRS6.detach();delay_(n);
 
   //////////////////////////// ТАКТ ПОВОРОТ КОРПУСА ОТН. ЛЕВОЙ НОГИ  //////////////////////////
myservoLB4.attach(4);myservoRB5.attach(5);
while((a4<=a04+db)||(a5<=a05+db))
  { myservoLB4.write(a4);myservoRB5.write(a5);a4=a4+n1;a5=a5+n1; delay_(n);}
myservoLB4.detach();myservoRB5.detach();delay_(n);
  goto metka1;}
/////////////////////////////////////////  ОСТАНОВКА   ///////////////////////////////////////
metka2:
  if(N==0&&f==0){f=1;
myservoLS3.attach(3);
myservoLB4.attach(4);
myservoRB5.attach(5);
myservoRS6.attach(6);
myservoLS3.write(a03); delay_(5*n);
myservoLB4.write(a04); delay_(5*n);
myservoRB5.write(a05); delay_(5*n);
myservoRS6.write(a06); delay_(5*n);
myservoLS3.detach();myservoLB4.detach();myservoRB5.detach(); myservoRS6.detach();
  }

/////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////  ХОЖДЕНИЕ НАЗАД //////////////////////////////////////
  metka3:
  if(N==2){if(f==1){myservoLS3.attach(3);myservoLB4.attach(4);myservoRB5.attach(5); myservoRS6.attach(6); }f=0;
  ////////////////////////////////   ТАКТ НАКЛОН ВПРАВО ///////////////////////////////////////
  myservoRS6.attach(6);
while((a3<=a03+da)||(a6<=a06+da))
  { myservoLS3.write(a3);myservoRS6.write(a6);a3=a3+n1;a6=a6+n1; delay_(n);}
  myservoLS3.detach();delay_(n);
 
   //////////////////////////// ТАКТ ПОВОРОТ КОРПУСА ОТН. ПРАВОЙ НОГИ  //////////////////////////
  myservoLB4.attach(4);myservoRB5.attach(5);
while((a4<=a04+db)||(a5<=a05+db))
  { myservoLB4.write(a4);myservoRB5.write(a5);a4=a4+n1;a5=a5+n1; delay_(n);}
myservoLB4.detach();myservoRB5.detach();delay_(n);

 
/////////////////////////////////   ТАКТ НАКЛОН ВЛЕВО  //////////////////////////////////////
myservoLS3.attach(3);
while ((a3>=a03-da)||(a6>=a06-da))
  { myservoLS3.write(a3);myservoRS6.write(a6);a3=a3-n1;a6=a6-n1; delay_(n);}
  myservoRS6.detach();delay_(n);
 
   //////////////////////////// ТАКТ ПОВОРОТ КОРПУСА ОТН. ЛЕВОЙ НОГИ  //////////////////////////
   myservoLB4.attach(4);myservoRB5.attach(5);
while((a4>=a04-db)||(a5>=a05-db))
  { myservoLB4.write(a4);myservoRB5.write(a5);a4=a4-n1;a5=a5-n1; delay_(n);}
myservoLB4.detach();myservoRB5.detach();delay_(n);

  goto metka3;}
}

////////////////////////////////////////   ФУНКЦИИ ОПИСАНИЕ   //////////////////////////////////////////////////////
//Функция заменяет выполнение команды delay(n); в ней на время паузы  идёт опрос датчика

void delay_(int T1)
{
Y2=millis();metka:Y1=millis();if(Y1-Y2>=T1){}
else { if (irrecv.decode(&results))         //Если произошло событие/кнопка была нажата
  {
   // Serial.println(results.value, HEX);//Выводим в монитор порта код нажатой кнопки в шестнадцатиричном виде
    switch ( results.value )
    {
    case 0x88877://кнопка "вверх-вперёд"
       N=1;
        break;
    case 0x8A857://кнопка "вниз-назад"
        N=2;
        break;
    case 0x8C837://кнопка "ОК-стоп"
       N=0;
        break;
    case 0x848B7://кнопка "влево"
       N=3;
        break;
    case 0x828D7://кнопка "вправо"
       N=4;
        break;
    }
     //Serial.println(N);
     irrecv.resume();//Считываем следующую значение/кнопку
   } goto metka;}

}

//////////////////////////    скетч для управления 8 сервоприводами квадропода   /////////////////
#include <Servo.h> //используем библиотеку

Servo myservoK3 ;  //указываем сервопривод заднего левого колена
Servo myservoB4;  //указываем сервопривод  заднего левого бедра
Servo myservoK5 ;  //указываем сервопривод переднего левого колена
Servo myservoB6;  //указываем сервопривод  переднего левого бедра
Servo myservoK7 ;  //указываем сервопривод заднего правого колена
Servo myservoB8;  //указываем сервопривод  заднего правого бедра
Servo myservoK9 ;  //указываем сервопривод переднего правого колена
Servo myservoB10;  //указываем сервопривод переднего правого бедра
int n=17;
int a03=110;int a04=75;int a05=55;int a06=97;int a07=80;int a08=75;int a09=73;int a010=85;//переменные для углов начального положения конечностей
int da=10;//переменная амплитуды изменения угла в градусах для колен
int db=15;//переменная амплитуды изменения угла в градусах для бёдер
int i;
int a3=a03;int a4=a04;int a5=a05;int a6=a06;int a7=a07;int a8=a08;int a9=a09;int a10=a010;// переменные для хранения значений текущих углов
//////при увеличении значения угла сервоприводы движут лапы :B4,B6-вперёд B8,B10-назад K3,K9-вниз K5,K7-вверх ////////
void setup()
{

myservoK3.attach(3);//выводы  для подключения 1-8 сервоприводов
myservoB4.attach(4);
myservoK5.attach(5);
myservoB6.attach(6);
myservoK7.attach(7);
myservoB8.attach(8);
myservoK9.attach(9);
myservoB10.attach(10);

myservoK3.write(a03); delay(n);// начальная установка-настройка углов поворота валов серв
myservoB4.write(a04); delay(n);
myservoK5.write(a05); delay(n);
myservoB6.write(a06); delay(n);
myservoK7.write(a07); delay(n);
myservoB8.write(a08); delay(n);
myservoK9.write(a09); delay(n);
myservoB10.write(a010); delay(n);


delay(250*n);
}

void loop()
{
  /////////////////////////////////////////////  ПРЯМОХОЖДЕНИЕ  /////////////////////////////////////////////
  ///////////// K5,K7 вверх///////////
   for(i = 0 ; i<=da; i++)
  {myservoK5.write(a5+i);
  myservoK7.write(a7+i);
  delay(n);}
  ///////////// B6,B8- вперёд B4,B10- назад ////////
   for(i = 0 ; i<=db; i++)
  {myservoB6.write(a6+i);
  myservoB8.write(a8-i);
  myservoB4.write(a4-i);
  myservoB10.write(a10+i);
  delay(n);}
  /////////////K5,K7 вниз /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK5.write(a5-i);
  myservoK7.write(a7-i);
  delay(n);}
  ////////////K3,K9 вверх /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK3.write(a3-i);
   myservoK9.write(a9-i);
  delay(n);}
  ///////////// B6,B8- назад B4,B10- вперёд ////////
  for(i = 0 ; i<=db; i++)
  {myservoB6.write(a6-i);
  myservoB8.write(a8+i);
  myservoB4.write(a4+i);
  myservoB10.write(a10-i);
  delay(n);}
  //////////////K3,K9 вниз /////////////
   for(i = 0 ; i<=da; i++)
  {myservoK3.write(a3+i);
   myservoK9.write(a9+i);
  delay(n);}
  /////////////////////////////////////////////////////////////////////////////////////////
 
}