roboforum.ru

slom писал(а):команды:

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

Код: Выделить всё • Развернуть

#include <PS2X_lib.h>  //for v1.6
#include <Servo.h>
PS2X ps2x;
int PS2 = 0; 
Servo LXservo;
Servo LYservo;
#define L_dir 2   //пины для драйвера мотора
#define L_pwm 3
#define R_dir 4
#define R_pwm 5

int mass[4];      //здесь будем хранить значения для скоростей и направлений
                  //mass[0]   лев.напр.
                  //mass[1]   лев.скор.
                  //mass[2]   прав.напр.
                  //mass[3]   прав.скор.
                 
void setup(){
  LXservo.attach(8);
  LYservo.attach(6);
  
  PS2 = ps2x.config_gamepad(13,11,10,12, true, true);   //Настройка выводов: (clock, command, attention, data, true, true)
  pinMode(L_dir, OUTPUT);
  pinMode(L_pwm, OUTPUT);
  pinMode(R_dir, OUTPUT);
  pinMode(R_pwm, OUTPUT);
}

void loop(){
  LXservo.write(map(ps2x.Analog(PSS_LX), 255, 0, 0, 180)); ; 
 delay(50); 
  ps2x.read_gamepad(false, 0);
  
  LYservo.write(map(ps2x.Analog(PSS_LY), 0, 255, 0, 90)); ; 
 delay(50); 
  ps2x.read_gamepad(false, 0);
  
  
    dir_pwm(ps2x.Analog(PSS_RY),2);   //обрабатываем правый джойстик
  digitalWrite(R_dir, mass[2]);
  analogWrite(R_pwm, mass[3]);
}    

void dir_pwm(int Y, int n){         //Подпрограмма для обработки значений джойстика
  if (Y == 128){
    mass[n+1] = 0;
  }
  if (Y >128){
    mass[n+1] = Y*2-255;
    mass[n] = 1;
  }
  if (Y <128){
    mass[n+1] = 255-Y*2;
    mass[n] = 0;
  }
}