Технический форум по робототехнике.
//ПАУК (ГЕКСАПОД) 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);}
}}}
/////////////
}
///////////////////////////////////////////////////////////////////////////////////////////////////////