Скетч управления роботом-пауком от джойстика PS2. Нужна помощь.

Юрец
Offline
Зарегистрирован: 20.01.2016

Доброго всем дня. Вот решил перейти в управлении роботом с ИК на джойстик PS2. Взял скетч управления танком jeka_tm и как мог приколхозил к своему. Предполагается, что робот должен управляться левым стиком. Но, при включении, робот занимает начальное положение (строки 047-058) и сразу начинает выполнение блока "назад" на джойстик не реагирует. Монитор порта выдаёт

@ OUT:IN Configure 1:FF 43:FF 0:FF 1:FF 0:FF OUT:IN Configure 1:FF 44:FF 0:FF 1:FF 3:FF 0:FF 0:FF 0:FF 0:FF OUT:IN Configure 1:FF 43:FF 0:FF 0:FF 5A:FF 5A:FF 5A:FF 5A:FF 5A:FF OUT:IN Configure 1:FF 43:FF 0:FF 1:FF 0:FF OUT:IN Configure 1:FF 44:FF 0:FF 1:FF 3:FF 0:FF 0:FF 0:FF 0:FF OUT:IN Configure 1:FF 43:FF 0:FF 0:FF 5A:FF 5A:FF 5A:FF 5A:FF 5A:FF OUT:IN Configure 1:FF 43:FF 0:FF 1:FF 0:FF OUT:IN Configure 1:FF 44:FF 0:FF 1:FF 3:FF 0:FF 0:FF 0:FF 0:FF OUT:IN Configure 1:FF 43:FF 0:FF 0:FF 5A:FF 5A:FF 5A:FF 5A:FF 5A:FF OUT:IN Configure 1:FF 43:FF 0:FF 1:FF 0:FF OUT:IN Configure 1:FF 44:FF 0:FF 1:FF 3:FF 0:FF 0:FF 0:FF 0:FF OUT:IN Configure 1:FF 43:FF 0:FF 0:FF 5A:FF 5A:FF 5A:FF 5A:FF 5A:FF OUT:IN Configure 1:FF 43:FF 0:FF 1:FF 0:FF OUT:IN Configure 1:FF 44:FF 0:FF 1:FF 3:FF 0:FF 0:FF 0:FF 0:FF OUT:IN Configure 1:FF 43:FF 0:FF 0:FF 5A:FF 5A:FF 5A:FF 5A:FF 5A:FF OUT:IN 1:FF 42:FF 0:FF 0:FF 0:FF 0:FF 0:FF 0:FF 0:FF 0:0 0:0 0:0 0:0 0:0 0:0 0:0 0:0 0:0 0:0 0:0 0:0 и повторяет это без изменений, не реагируя на стик. Питание приёмника 3,3В, подключил вроде правильно плата Мега 2560. Скажите, пожалуйста, где я ошибся?

#include <PS2X_lib.h>
#include <Servo.h>

PS2X ps2x; // Создаём объект для библиотеки джойстика

int error = 0; 
byte type = 0;
byte vibrate = 0;
byte UP1;
boolean DOWN1;
byte LEFT1;
boolean RIGHT1;

Servo servoLG1; // Создаём объекты для библиотеки Сервы
Servo servoLV1;
Servo servoLG2;
Servo servoLV2;
Servo servoLG3;
Servo servoLV3;
Servo servoRG1;
Servo servoRV1;
Servo servoRG2;
Servo servoRV2;
Servo servoRG3;
Servo servoRV3;

unsigned long currentTime2; //Переменная для pause
byte var = 0;
byte run = 0; // Переменная для движения

void setup()
{
  error = ps2x.config_gamepad(8,6,5,7, true, true);   //Настройка выводов: (clock, command, attention, data, Pressures?, Rumble?)
  servoLG1.attach(26);  // Подключаем сервы к пинам
  servoLV1.attach(34);
  servoLG2.attach(27);
  servoLV2.attach(35);
  servoLG3.attach(28);
  servoLV3.attach(36);
  servoRG1.attach(30);
  servoRV1.attach(38);
  servoRG2.attach(31);
  servoRV2.attach(39);
  servoRG3.attach(32);
  servoRV3.attach(40);
  Serial.begin(9600); // устанавливает скорость обмена данными с компьютером
  servoLG1.write(90); // 1-я нога начальное положение после включения
  servoLG2.write(90); // 2-я нога
  servoLG3.write(90); // 3-я нога
  servoRG1.write(90); // 4-я нога
  servoRG2.write(90); // 1-я нога
  servoRG3.write(90); // 2-я нога
  servoLV1.write(90); // 3-я нога
  servoLV2.write(90); // 4-я нога
  servoLV3.write(90); // 3-я нога
  servoRV1.write(90); // 4-я нога
  servoRV2.write(90); // 3-я нога
  servoRV3.write(90); // 4-я нога
}

//Функция паузы
int pause(int y)
{
  bool x = 0;
  if (millis() - currentTime2 >= y) x = 1, currentTime2 = millis();
  return x;
}

void loop() {

ps2x.read_gamepad(false, vibrate);
  //===========================================================================================
  //                                        Граничные условия для X и Y
  //===========================================================================================

  if (ps2x.Analog(PSS_LX) == 128){
    LEFT1 = 0;
  } 
  if (ps2x.Analog(PSS_LX) < 128){
    LEFT1 = 255-ps2x.Analog(PSS_LX)*2;
    RIGHT1 = 0;
  }
  if (ps2x.Analog(PSS_LX) > 128){
    LEFT1 = ps2x.Analog(PSS_LX)*2-255;
    RIGHT1 = 1;
  }    
  if (ps2x.Analog(PSS_LY) == 128){
    UP1 = 0;
  } 
  if (ps2x.Analog(PSS_LY) < 128){
    UP1 = 255-ps2x.Analog(PSS_LY)*2;
    DOWN1 = 1;
  }
  if (ps2x.Analog(PSS_LY) > 128){
    UP1 = ps2x.Analog(PSS_LY)*2-255;
    DOWN1 = 0;
  }
  
      //===========================================================================================
      //                                        Вперед1
      //===========================================================================================  
      if (UP1 > 0 && DOWN1 == 1)
      {
        run = 2;
      }
      //===========================================================================================
      //                                        Назад1
      //===========================================================================================
      if (UP1 > 0 && DOWN1 == 0)
      {
        run = 1;
      }
      //===========================================================================================
      //                                        Разворот влево1
      //===========================================================================================
      if (LEFT1 > 0 && UP1 == 0 && RIGHT1 == 0)
      {
        run = 3;
      }
      //===========================================================================================
      //                                        Разворот вправо1
      //===========================================================================================
      if (LEFT1 > 0 && UP1 == 0 && RIGHT1 == 1)
      {
        run = 4;
      }
      //===========================================================================================
      //                                        Остановка
      //===========================================================================================
            if (UP1 == 0 && DOWN1 == 0)
      {
              run = 0;
        var = 0;
        servoLG1.write(90); // 1-я нога
        servoLG2.write(90); // 2-я нога
        servoLG3.write(90); // 3-я нога
        servoRG1.write(90); // 4-я нога
        servoRG2.write(90); // 1-я нога
        servoRG3.write(90); // 2-я нога
        servoLV1.write(90); // 3-я нога
        servoLV2.write(90); // 4-я нога
        servoLV3.write(90); // 3-я нога
        servoRV1.write(90); // 4-я нога
        servoRV2.write(90); // 3-я нога
        servoRV3.write(90); // 4-я нога
      }
        
   if (run == 1)
  {
    //Посылаем запрос в функцию паузы- если 1 выполняем действие по очереди с паузой 200, при этом можно без тормозов опрашивать другие датчики
    if (pause (200))
    {
      switch (var) {
        case 0:
          servoLV1.write(120); // 1-я нога вверх
          servoLG1.write(60); // 1-я нога вперёд
          servoLV3.write(60); // 3-я нога вверх
          servoLG3.write(60); // 3-я нога вперёд
          servoRV2.write(60); // 2-я нога вверх
          servoRG2.write(120); // 2-я нога вперёд
          servoLG2.write(120); // 1-я нога вперёд
          servoRG1.write(60); // 3-я нога вперёд
          servoRG3.write(60); // 2-я нога вперёд
          var = 1;
          break;
        case 1:
          servoLV1.write(90); // 3-я нога вниз
          servoLV3.write(90); // 3-я нога вверх
          servoRV2.write(90); // 2-я нога вверх
          var = 2;
          break;
        case 2:
          servoRV1.write(60); // 1-я нога вверх
          servoRG1.write(120); // 1-я нога вперёд
          servoRV3.write(120); // 3-я нога вверх
          servoRG3.write(120); // 3-я нога вперёд
          servoLV2.write(120); // 2-я нога вверх
          servoLG2.write(60); // 2-я нога вперёд
          servoRG2.write(60); // 1-я нога вперёд
          servoLG1.write(120); // 3-я нога вперёд
          servoLG3.write(120); // 2-я нога вперёд
          var = 3;
          break;
        case 3:
          servoRV1.write(90); // 3-я нога вниз
          servoRV3.write(90); // 3-я нога вверх
          servoLV2.write(90); // 2-я нога вверх
          var = 0;
          break;
    }
   }
  }
   if (run == 2)
  {
    //Посылаем запрос в функцию паузы- если 1 выполняем действие по очереди с паузой 200, при этом можно без тормозов опрашивать другие датчики
    if (pause (200))
    {
      switch (var) {
        case 0:
          servoLV1.write(120); // 1-я нога вверх
          servoLG1.write(120); // 1-я нога вперёд
          servoLV3.write(60); // 3-я нога вверх
          servoLG3.write(120); // 3-я нога вперёд
          servoRV2.write(60); // 2-я нога вверх
          servoRG2.write(60); // 2-я нога вперёд
          servoLG2.write(60); // 1-я нога вперёд
          servoRG1.write(120); // 3-я нога вперёд
          servoRG3.write(120); // 2-я нога вперёд
          var = 1;
          break;
        case 1:
          servoLV1.write(90); // 3-я нога вниз
          servoLV3.write(90); // 3-я нога вверх
          servoRV2.write(90); // 2-я нога вверх
          var = 2;
          break;
        case 2:
          servoRV1.write(60); // 1-я нога вверх
          servoRG1.write(60); // 1-я нога вперёд
          servoRV3.write(120); // 3-я нога вверх
          servoRG3.write(60); // 3-я нога вперёд
          servoLV2.write(120); // 2-я нога вверх
          servoLG2.write(120); // 2-я нога вперёд
          servoRG2.write(120); // 1-я нога вперёд
          servoLG1.write(60); // 3-я нога вперёд
          servoLG3.write(60); // 2-я нога вперёд
          var = 3;
          break;
        case 3:
          servoRV1.write(90); // 3-я нога вниз
          servoRV3.write(90); // 3-я нога вверх
          servoLV2.write(90); // 2-я нога вверх
          var = 0;
          break;
      }
    }
  }
  if (run == 3)
  {
    //Посылаем запрос в функцию паузы- если 1 выполняем действие по очереди с паузой 200, при этом можно без тормозов опрашивать другие датчики
    if (pause (200))
    {
      switch (var) {
        case 0:
          servoLV1.write(120); // 1-я нога вверх
          servoLG1.write(120); // 1-я нога вперёд
          servoLV3.write(60); // 3-я нога вверх
          servoLG3.write(120); // 3-я нога вперёд
          servoRV2.write(60); // 2-я нога вверх
          servoRG2.write(120); // 2-я нога вперёд
          servoLG2.write(60); // 1-я нога вперёд
          servoRG1.write(60); // 3-я нога вперёд
          servoRG3.write(120); // 2-я нога вперёд
          var = 1;
          break;
        case 1:
          servoLV1.write(90); // 3-я нога вниз
          servoLV3.write(90); // 3-я нога вверх
          servoRV2.write(90); // 2-я нога вверх
          var = 2;
          break;
        case 2:
          servoRV1.write(60); // 1-я нога вверх
          servoRG1.write(120); // 1-я нога вперёд
          servoRV3.write(120); // 3-я нога вверх
          servoRG3.write(60); // 3-я нога вперёд
          servoLV2.write(120); // 2-я нога вверх
          servoLG2.write(120); // 2-я нога вперёд
          servoRG2.write(60); // 1-я нога вперёд
          servoLG1.write(60); // 3-я нога вперёд
          servoLG3.write(60); // 2-я нога вперёд
          var = 3;
          break;
        case 3:
          servoRV1.write(90); // 3-я нога вниз
          servoRV3.write(90); // 3-я нога вверх
          servoLV2.write(90); // 2-я нога вверх
          var = 0;
          break;
      }
    }
  }
   if (run == 4)
  {
    //Посылаем запрос в функцию паузы- если 1 выполняем действие по очереди с паузой 200, при этом можно без тормозов опрашивать другие датчики
    if (pause (200))
    {
      switch (var) {
        case 0:
          servoLV1.write(120); // 1-я нога вверх
          servoLG1.write(60); // 1-я нога вперёд
          servoLV3.write(60); // 3-я нога вверх
          servoLG3.write(60); // 3-я нога вперёд
          servoRV2.write(60); // 2-я нога вверх
          servoRG2.write(60); // 2-я нога вперёд
          servoLG2.write(120); // 1-я нога вперёд
          servoRG1.write(120); // 3-я нога вперёд
          servoRG3.write(120); // 2-я нога вперёд
          var = 1;
          break;
        case 1:
          servoLV1.write(90); // 3-я нога вниз
          servoLV3.write(90); // 3-я нога вверх
          servoRV2.write(90); // 2-я нога вверх
          var = 2;
          break;
        case 2:
          servoRV1.write(60); // 1-я нога вверх
          servoRG1.write(60); // 1-я нога вперёд
          servoRV3.write(120); // 3-я нога вверх
          servoRG3.write(60); // 3-я нога вперёд
          servoLV2.write(120); // 2-я нога вверх
          servoLG2.write(60); // 2-я нога вперёд
          servoRG2.write(120); // 1-я нога вперёд
          servoLG1.write(120); // 3-я нога вперёд
          servoLG3.write(120); // 2-я нога вперёд
          var = 3;
          break;
        case 3:
          servoRV1.write(90); // 3-я нога вниз
          servoRV3.write(90); // 3-я нога вверх
          servoLV2.write(90); // 2-я нога вверх
          var = 0;
          break;
      }
    }
  }
 }







 

jeka_tm
jeka_tm аватар
Offline
Зарегистрирован: 19.05.2013

а ты пульт сначала проверял?

Юрец
Offline
Зарегистрирован: 20.01.2016

А чем его лучше проверить? Я в Уно что-то залил для проверки, вроде в мониторе порта есть изменения при нажатии. Джойстик с приёмником вроде вяжутся (перестают зелёные диоды мигать при включении обоих). Какой тест лучше использовать?

jeka_tm
jeka_tm аватар
Offline
Зарегистрирован: 19.05.2013

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

Юрец
Offline
Зарегистрирован: 20.01.2016

Буду пробывать

Юрец
Offline
Зарегистрирован: 20.01.2016

Сам джойстик поборол. 3,3В с Меги приёмник не запускают, а вот от 5В работает. Теперь буду скетч пилить, кое-что начинаю догонять.

jeka_tm
jeka_tm аватар
Offline
Зарегистрирован: 19.05.2013

только от 5в сдохнуть у тебя может приемник. и где ты новый возьмешь? они вроде как друг на друга запрограммированы. и другой не подойдет. ставь лучше отдельный стабилизатор

Юрец
Offline
Зарегистрирован: 20.01.2016

Ур-ра догнал я всё-таки своими куриными мозгами! Поправил скетч и всё заработало! Спасибо jeka_tm! Всегда помогаешь и наставляешь на путь истинный! А на счёт стабилизатора, буду искать, спасибо.

jeka_tm
jeka_tm аватар
Offline
Зарегистрирован: 19.05.2013

видео выложи как бегает

Юрец
Offline
Зарегистрирован: 20.01.2016

Стабилизатор привинчу и выложу.

555max
Offline
Зарегистрирован: 15.07.2018

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

в скейче

555max
Offline
Зарегистрирован: 15.07.2018

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