Robô Arduino com controlador PS2 (joystick para PlayStation 2)

Aplicativos e serviços online

Arduino IDE

Sobre este projeto

Etapa 1:Ferramentas e materiais

• Ferro e fio de solda (link / link). Os motores DC já vinham com fios soldados aos seus terminais ... Mas eventualmente ele irá quebrar e você terá que vendê-lo novamente. Portanto, considere ter um bom ferro de solda e fio à mão.
• Folha de espuma EVA (ou outro material não condutor). O chassi do robô que usei neste projeto é feito de alumínio e as placas de circuito são instaladas nessas peças de metal. Usei uma camada de folha de espuma entre as placas e a placa de metal para evitar possíveis curtos-circuitos.
• Fita dupla-face . Foi usado para colar as folhas de espuma nas placas de circuito e para a instalação do modo H-Bridge.
• Tesouras , para cortar alguns retângulos de folha de espuma.

• placa de desenvolvimento baseada em Arduino Uno (link / link / link / link). É usado como o controlador principal do robô. É realmente fácil de usar e programar com o Arduino IDE, ótimo para iniciantes em eletrônica e programação.

• Módulo ponte H de canal duplo L298N (link / link / link / link). Este módulo permite que os sinais de 3,3 V do Wemos (ou de um Arduino) sejam amplificados para os 12 V necessários para os motores.

• DIY Robot Chassis Tank (link). Este incrível kit tem tudo que você precisa para construir um tanque:dois motores DC, engrenagens, esteiras, parafusos, porcas, etc. Já vem com as ferramentas necessárias para montar o chassi, o que é ótimo para iniciantes!

• Controle remoto sem fio PS2 (link). Este controlador de videogame pode enviar comandos sem fio para um receptor, que pode interagir com um microcontrolador usando comunicação serial.

• baterias 18650 3,7 V (x3) (link / link). Eu costumava alimentar todo o circuito. Este tanque usa motores de 12V. Usei três baterias de 3,7 V em série para alimentá-los.

• suporte de bateria 3S 18650 (link / link / link). Ele pode conter três baterias 18650 em série e pode ser facilmente instalado na parte de trás do tanque.

• carregador de bateria 18650 (link / link). Eventualmente, suas baterias ficarão sem carga. Quando isso acontecer, um carregador de bateria virá em seu socorro.
• Jumpers (link / link). Usei 6 jumpers macho-fêmea para sinais entre a ponte h e Wemos, e 2 jumpers macho-fêmea para 5V e Gnd. Você pode precisar de mais se planeja adicionar alguns sensores.
• Cabo USB Tipo B . Você precisará disso para enviar seu código. A maioria das placas já vem com seu próprio cabo.

Etapa 2:montagem do robô

Etapa 3:Instalação do circuito

• Pino digital 3 => Pino ENA H-Bridge
• Pino digital 5 => pino H-Bridge IN1
• Pino digital 4 => pino H-Bridge IN2
• Pino digital 8 => Pino H-Bridge IN3
• Pino digital 7 => pino H-Bridge IN4
• Pino digital 6 => Pino ENB H-Bridge
• 5V pin => H-Bridge 5V pin
• Gnd pin => H-bridge Gnd pin
• Pino digital 10 => pino 6 do receptor PS2
• Pino digital 11 => pino 2 do receptor PS2
• Pino digital 12 => pino 1 do receptor PS2
• Pino digital 13 => Pino 7 do receptor PS2
• 3,3 V pin => PS2 receptor pino 5
• Gnd pin => receptor PS2 pino 4

• ENA pin => Arduino digital pin 3
• IN1 pin => Arduino digital pin 5
• IN2 pin => Arduino digital pin 4
• IN3 pin => Arduino digital pin 8
• IN4 pin => Arduino digital pin 7
• ENB pin => Arduino digital pin 6
• 5V pin => Arduino 5V pin
• Gnd pin => Arduino Gnd pin
• Gnd pin => Fio negativo da bateria
• 12V pin => Fio positivo da bateria
• OUT1 => Fio negativo do motor direito
• OUT2 => Fio positivo do motor direito
• OUT3 => Fio positivo do motor esquerdo
• OUT4 => Fio negativo do motor esquerdo

• Pino 1 (dados) => pino digital 12 do Arduino
• Pino 2 (comando) => pino digital Arduino 11
• Pino 4 (solo) => pino Gnd do Arduino
• Pino 5 (alimentação) => pino Arduino 3.3V
• Pino 6 (atenção) => pino digital 10 do Arduino
• Pino 7 (relógio) => pino digital Arduino 13

Etapa 4:configurar o IDE do Arduino

Etapa 5:Biblioteca de controladores sem fio PS2 explicada

  PS2X ps2x;  

  erro =ps2x.config_gamepad (relógio, comando, atenção, dados, pressão ?, ruído?);  

  ps2x.Button (botão);  

• PSB_PAD_UP =botão direcional para cima;
• PSB_PAD_DOWN =botão direcional para baixo;
• PSB_PAD_LEFT =botão esquerdo do D-pad
• PSB_PAD_RIGHT =botão direito do D-pad

• PSB_GREEN ou PSB_TRIANGLE =botão do triângulo verde
• PSB_RED ou PSB_CIRCLE =botão do círculo vermelho
• PSB_BLUE ou PSB_CROSS =botão azul x
• PSB_PINK ou PSB_SQUARE =​​botão quadrado rosa

• PSB_L1 =botão disparador 1 do lado esquerdo
• PSB_R1 =botão disparador 1 do lado direito
• PSB_L2 =botão disparador 2 do lado esquerdo
• PSB_R2 =botão disparador 2 do lado direito

• PSB_L3 =botão de pressão do manípulo esquerdo (sim, você pode pressionar o manche como um botão de pressão)
• PSB_R3 =botão do manípulo direito

• PSB_SELECT =botão de seleção
• PSB_START =botão iniciar

  ps2x.Analog (stick_axis);  

• PSS_LY =posição y do controle esquerdo
• PSS_LX =posição x do controle esquerdo
• PSS_RY =posição y do manípulo direito
• PSS_RX =posição x do manípulo direito

Etapa 6:Esboço nº 1 - Botões do Dpad

• PSB_PAD_UP =avançar
• PSB_PAD_DOWN =retroceder
• PSB_PAD_LEFT =vire à esquerda
• PSB_PAD_RIGHT =vire à direita

• Reinicialize a placa Arduino Uno. Tente várias vezes (para mim, geralmente funciona após a terceira reinicialização);
• Verifique se o joystick está ligado (e com as baterias carregadas). O receptor possui alguns LEDs que indicam se o joystick foi emparelhado. Verifique também se os pinos RX e TX do Arduino piscam ao receber novos comandos (e atualiza o status na porta serial);
• Verifique as conexões ... pode haver algo errado nos jumpers entre os componentes.

Código explicado:

  #include   

  // Estes são usados ​​para definir a direção do driver de ponte. #define ENA 3 // ENA #define MOTORA_1 4 // IN3 #define MOTORA_2 5 // IN4 #define MOTORB_1 8 // IN1 #define MOTORB_2 7 // IN2 #define ENB 6 // ENB  

  // Configure os pinos de saída pinMode (ENA, OUTPUT); pinMode (MOTORA_1, SAÍDA); pinMode (MOTORA_2, SAÍDA); pinMode (ENB, OUTPUT); pinMode (MOTORB_1, SAÍDA); pinMode (MOTORB_2, SAÍDA); // Desabilita ambos os motores digitalWrite (ENA, 0); digitalWrite (ENB, 0);  

  // Inicia a comunicação serial Serial.begin (57600);  

  =ps2x.config_gamepad (13,11,10,12, verdadeiro, verdadeiro); // configurar pinos e configurações:GamePad (relógio, comando, atenção, dados, pressões ?, Rumble?) verificar se há erros // Verificar se há erros se (erro ==0) {Serial.println ("Controlador encontrado, configurado com sucesso" ); } else if (erro ==1) Serial.println ("Nenhum controlador encontrado, verifique a fiação ou reinicie o Arduino"); else if (erro ==2) Serial.println ("Controlador encontrado, mas não aceita comandos"); else if (erro ==3) Serial.println ("Controlador se recusando a entrar no modo Pressões, pode não suportá-lo."); // Verifique o tipo de controlador type =ps2x.readType (); switch (tipo) {case 0:Serial.println ("Tipo de controlador desconhecido"); pausa; caso 1:Serial.println ("Controlador DualShock encontrado"); pausa; caso 2:Serial.println ("GuitarHero Controller Found"); pausa; }  

  // AVANÇAR if (ps2x.Button (PSB_PAD_UP)) {digitalWrite (MOTORA_1, BAIXO); digitalWrite (MOTORA_2, HIGH); digitalWrite (MOTORB_1, HIGH); digitalWrite (MOTORB_2, LOW); analogWrite (ENB, 1023); analogWrite (ENA, 1023); Serial.println ("Avançar"); }  

Etapa 7:Esboço 2 - Stick analógico e botões digitais (L2 e R2)

  int motor_right_speed =0; 
 int motor_left_speed =0;  

  int nJoyL =ps2x.Analog (PSS_LX); // ler o manípulo esquerdo nJoyL =map (nJoyL, 0, 255, 1023, -1023); motor_right_speed =1023; motor_left_speed =1023; if (nJoyL <0) {motor_right_speed =motor_right_speed + nJoyL; } if (nJoyL> 0) {motor_left_speed =motor_left_speed - nJoyL; }  

  if (ps2x.Button (PSB_R2)) {digitalWrite (MOTORA_1, LOW); digitalWrite (MOTORA_2, HIGH); digitalWrite (MOTORB_1, HIGH); digitalWrite (MOTORB_2, LOW); analogWrite (ENA, motor_right_speed); analogWrite (ENB, motor_left_speed); } if (ps2x.Button (PSB_L2)) {digitalWrite (MOTORA_1, HIGH); digitalWrite (MOTORA_2, LOW); digitalWrite (MOTORB_1, LOW); digitalWrite (MOTORB_2, HIGH); analogWrite (ENA, motor_right_speed); analogWrite (ENB, motor_left_speed); } if (! ps2x.Button (PSB_L2) ​​&&! ps2x.Button (PSB_R2)) {analogWrite (ENA, 0); analogWrite (ENB, 0); }  

Etapa 8:Esboço nº 3 - Sticks analógicos esquerdo e direito (unidade diferencial)

  int nJoyL =ps2x.Analog (PSS_LY); // lê o manípulo esquerdo int nJoyR =ps2x.Analog (PSS_RY); // ler o manípulo direito nJoyL =map (nJoyL, 0, 255, 1023, -1023); nJoyR =mapa (nJoyR, 0, 255, -1023, 1023);  

  // Executa movimentos com base em ambas as alavancas analógicas if (nJoyR> 50) {digitalWrite (MOTORA_1, HIGH); digitalWrite (MOTORA_2, LOW); analogWrite (ENA, 1023); } if (nJoyR <-50) {digitalWrite (MOTORA_1, BAIXO); digitalWrite (MOTORA_2, HIGH); analogWrite (ENA, 1023); } if (abs (nJoyR) <50) {analogWrite (ENA, 0); }  

Etapa 9:Esboço nº 4 - Somente analógico esquerdo

Etapa 10:Considerações finais

Código

• Esboço # 1
• Sketch # 2
• Sketch # 3
• Sketch # 4

Sketch # 1 Arduino

 // PS2 Tank por Igor Fonseca @ 2019 // Controla um tanque robótico usando um joystick PS2, usando botões direcional // baseado em um exemplo usando a biblioteca PS2X de Bill Porter 2011 // Todo o texto acima deve ser incluído em qualquer redistribuição.// incluir bibliotecas # incluir  // Elas são usadas para definir a direção do driver da ponte. # define ENA 3 // ENA # define MOTORA_1 4 // IN3 # define MOTORA_2 5 // IN4 #define MOTORB_1 8 // IN1 # define MOTORB_2 7 // IN2 # define ENB 6 // ENBPS2X ps2x; // criar classe de controlador PS2 // agora, a biblioteca NÃO suporta controladores hot pluggable, o que significa // você deve sempre reiniciar seu Arduino após conectar o controlador, // ou chamar config_gamepad (pinos) novamente após conectar o controlador. erro interno =0; byte type =0; byte vibrate =0; void setup () {// Configure os pinos de saída pinMode (ENA, OUTPUT); pinMode (MOTORA_1, SAÍDA); pinMode (MOTORA_2, SAÍDA); pinMode (ENB, OUTPUT); pinMode (MOTORB_1, SAÍDA); pinMode (MOTORB_2, SAÍDA); // Desabilita ambos os motores digitalWrite (ENA, 0); digitalWrite (ENB, 0); // Inicia a comunicação serial Serial.begin (57600); erro =ps2x.config_gamepad (13,11,10,12, verdadeiro, verdadeiro); // configurar pinos e configurações:GamePad (relógio, comando, atenção, dados, pressões ?, Rumble?) verificar se há erros // Verificar se há erros se (erro ==0) {Serial.println ("Controlador encontrado, configurado com sucesso" ); } else if(error ==1) Serial.println("No controller found, check wiring or reset the Arduino"); else if(error ==2) Serial.println("Controller found but not accepting commands"); else if(error ==3) Serial.println("Controller refusing to enter Pressures mode, may not support it."); // Check for the type of controller type =ps2x.readType(); switch(type) { case 0:Serial.println("Unknown Controller type"); pausa; case 1:Serial.println("DualShock Controller Found"); pausa; case 2:Serial.println("GuitarHero Controller Found"); pausa; }}// Main loopvoid loop(){ if(error ==1) //skip loop if no controller found return; else { //DualShock Controller ps2x.read_gamepad(false, vibrate); // disable vibration of the controller // Perform movements based on D-pad buttons // MOVE FORWARD if(ps2x.Button(PSB_PAD_UP)) { digitalWrite(MOTORA_1,LOW); digitalWrite(MOTORA_2,HIGH); digitalWrite(MOTORB_1,HIGH); digitalWrite(MOTORB_2,LOW); analogWrite(ENB, 1023); analogWrite(ENA, 1023); Serial.println("Move forward"); } // TURN RIGHT if(ps2x.Button(PSB_PAD_RIGHT)){ digitalWrite(MOTORA_1,HIGH); digitalWrite(MOTORA_2,LOW); digitalWrite(MOTORB_1,HIGH); digitalWrite(MOTORB_2,LOW); analogWrite(ENB, 1023); analogWrite(ENA, 1023); Serial.println("Turn right"); } // TURN LEFT if(ps2x.Button(PSB_PAD_LEFT)){ digitalWrite(MOTORA_1,LOW); digitalWrite(MOTORA_2,HIGH); digitalWrite(MOTORB_1,LOW); digitalWrite(MOTORB_2,HIGH); analogWrite(ENB, 1023); analogWrite(ENA, 1023); Serial.println("Turn left"); } // MOVE BACK if(ps2x.Button(PSB_PAD_DOWN)){ digitalWrite(MOTORA_1,HIGH); digitalWrite(MOTORA_2,LOW); digitalWrite(MOTORB_1,LOW); digitalWrite(MOTORB_2,HIGH); analogWrite(ENB, 1023); analogWrite(ENA, 1023); Serial.println("Move back"); } if (!ps2x.Button(PSB_PAD_DOWN) &&!ps2x.Button(PSB_PAD_UP) &&!ps2x.Button(PSB_PAD_RIGHT) &&!ps2x.Button(PSB_PAD_LEFT)) { analogWrite(ENB, 0); analogWrite(ENA, 0); } delay(50); }} 

Sketch #2Arduino

// PS2 Tank by Igor Fonseca @2019// Controls a robotic tank using a PS2 joystick, using D-pad buttons// based on an example using the PS2X library by Bill Porter 2011// All text above must be included in any redistribution.// include libraries#include // These are used to set the direction of the bridge driver.#define ENA 3 //ENA#define MOTORA_1 4 //IN3#define MOTORA_2 5 //IN4#define MOTORB_1 8 //IN1#define MOTORB_2 7 //IN2#define ENB 6 //ENBint motor_right_speed =0;int motor_left_speed =0;PS2X ps2x; // create PS2 Controller Class//right now, the library does NOT support hot pluggable controllers, meaning //you must always either restart your Arduino after you conect the controller, //or call config_gamepad(pins) again after connecting the controller.int error =0; byte type =0;byte vibrate =0;void setup(){ // Configure output pins pinMode(ENA, OUTPUT); pinMode(MOTORA_1, OUTPUT); pinMode(MOTORA_2, OUTPUT); pinMode(ENB, OUTPUT); pinMode(MOTORB_1, OUTPUT); pinMode(MOTORB_2, OUTPUT); // Disable both motors digitalWrite(ENA,0); digitalWrite(ENB,0); // Start serial communication Serial.begin(57600); error =ps2x.config_gamepad(13,11,10,12, true, true); //setup pins and settings:GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error // Check for error if(error ==0){ Serial.println("Found Controller, configured successful"); } else if(error ==1) Serial.println("No controller found, check wiring or reset the Arduino"); else if(error ==2) Serial.println("Controller found but not accepting commands"); else if(error ==3) Serial.println("Controller refusing to enter Pressures mode, may not support it."); // Check for the type of controller type =ps2x.readType(); switch(type) { case 0:Serial.println("Unknown Controller type"); pausa; case 1:Serial.println("DualShock Controller Found"); pausa; case 2:Serial.println("GuitarHero Controller Found"); pausa; }}// Main loopvoid loop(){ if(error ==1) //skip loop if no controller found return; else { //DualShock Controller ps2x.read_gamepad(false, vibrate); // disable vibration of the controller int nJoyL =ps2x.Analog(PSS_LX); // read left stick nJoyL =map(nJoyL, 0, 255, 1023, -1023); int motor_right_speed =1023; int motor_left_speed =1023; if (nJoyL <0) { motor_right_speed =motor_right_speed + nJoyL; } if (nJoyL> 0) { motor_left_speed =motor_left_speed - nJoyL; } if(ps2x.Button(PSB_R2)) { digitalWrite(MOTORA_1,LOW); digitalWrite(MOTORA_2,HIGH); digitalWrite(MOTORB_1,HIGH); digitalWrite(MOTORB_2,LOW); analogWrite(ENA, motor_right_speed); analogWrite(ENB, motor_left_speed); } if(ps2x.Button(PSB_L2)) { digitalWrite(MOTORA_1,HIGH); digitalWrite(MOTORA_2,LOW); digitalWrite(MOTORB_1,LOW); digitalWrite(MOTORB_2,HIGH); analogWrite(ENA, motor_right_speed); analogWrite(ENB, motor_left_speed); } if(!ps2x.Button(PSB_L2) &&!ps2x.Button(PSB_R2)) { analogWrite(ENA, 0); analogWrite(ENB, 0); } delay(50); }} 

Sketch #3Arduino

// PS2 Tank by Igor Fonseca @2019// Controls a robotic tank using a PS2 joystick, using D-pad buttons// based on an example using the PS2X library by Bill Porter 2011// All text above must be included in any redistribution.// include libraries#include // These are used to set the direction of the bridge driver.#define ENA 3 //ENA#define MOTORA_1 4 //IN3#define MOTORA_2 5 //IN4#define MOTORB_1 8 //IN1#define MOTORB_2 7 //IN2#define ENB 6 //ENBPS2X ps2x; // create PS2 Controller Class//right now, the library does NOT support hot pluggable controllers, meaning //you must always either restart your Arduino after you conect the controller, //or call config_gamepad(pins) again after connecting the controller.int error =0; byte type =0;byte vibrate =0;void setup(){ // Configure output pins pinMode(ENA, OUTPUT); pinMode(MOTORA_1, OUTPUT); pinMode(MOTORA_2, OUTPUT); pinMode(ENB, OUTPUT); pinMode(MOTORB_1, OUTPUT); pinMode(MOTORB_2, OUTPUT); // Disable both motors digitalWrite(ENA,0); digitalWrite(ENB,0); // Start serial communication Serial.begin(57600); error =ps2x.config_gamepad(13,11,10,12, true, true); //setup pins and settings:GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error // Check for error if(error ==0){ Serial.println("Found Controller, configured successful"); } else if(error ==1) Serial.println("No controller found, check wiring or reset the Arduino"); else if(error ==2) Serial.println("Controller found but not accepting commands"); else if(error ==3) Serial.println("Controller refusing to enter Pressures mode, may not support it."); // Check for the type of controller type =ps2x.readType(); switch(type) { case 0:Serial.println("Unknown Controller type"); pausa; case 1:Serial.println("DualShock Controller Found"); pausa; case 2:Serial.println("GuitarHero Controller Found"); pausa; }}// Main loopvoid loop(){ if(error ==1) //skip loop if no controller found return; else { //DualShock Controller ps2x.read_gamepad(false, vibrate); // disable vibration of the controller int nJoyL =ps2x.Analog(PSS_LY); // read left stick int nJoyR =ps2x.Analog(PSS_RY); // read right stick nJoyL =map(nJoyL, 0, 255, 1023, -1023); nJoyR =map(nJoyR, 0, 255, -1023, 1023); // Perform movements based on both analog sticks if(nJoyR>50) { digitalWrite(MOTORA_1,HIGH); digitalWrite(MOTORA_2,LOW); analogWrite(ENA, 1023); } if(nJoyR<-50) { digitalWrite(MOTORA_1,LOW); digitalWrite(MOTORA_2,HIGH); analogWrite(ENA, 1023); } if (abs(nJoyR)<50) { analogWrite(ENA, 0); } if(nJoyL>50) { digitalWrite(MOTORB_1,HIGH); digitalWrite(MOTORB_2,LOW); analogWrite(ENB, 1023); } if(nJoyL<-50) { digitalWrite(MOTORB_1,LOW); digitalWrite(MOTORB_2,HIGH); analogWrite(ENB, 1023); } if (abs(nJoyL)<50) { analogWrite(ENB, 0); } delay(50); }} 

Sketch #4Arduino

// PS2 Tank by Igor Fonseca @2019// Controls a robotic tank using a PS2 joystick, using left analog stick// based on an example using the PS2X library by Bill Porter 2011// All text above must be included in any redistribution.// include libraries#include  //for v1.6// These are used to set the direction of the bridge driver.#define ENA 3 //ENA#define MOTORA_1 4 //IN3#define MOTORA_2 5 //IN4#define MOTORB_1 8 //IN1#define MOTORB_2 7 //IN2#define ENB 6 //ENBint motor_right_speed =0;int motor_left_speed =0;PS2X ps2x; // create PS2 Controller Class//right now, the library does NOT support hot pluggable controllers, meaning //you must always either restart your Arduino after you conect the controller, //or call config_gamepad(pins) again after connecting the controller.int error =0; byte type =0;byte vibrate =0;void setup(){ // Configure output pins pinMode(ENA, OUTPUT); pinMode(MOTORA_1, OUTPUT); pinMode(MOTORA_2, OUTPUT); pinMode(ENB, OUTPUT); pinMode(MOTORB_1, OUTPUT); pinMode(MOTORB_2, OUTPUT); // Disable both motors digitalWrite(ENA,0); digitalWrite(ENB,0); // Start serial communication Serial.begin(57600); error =ps2x.config_gamepad(13,11,10,12, true, true); //setup pins and settings:GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error // Check for error if(error ==0){ Serial.println("Found Controller, configured successful"); } else if(error ==1) Serial.println("No controller found, check wiring or reset the Arduino"); else if(error ==2) Serial.println("Controller found but not accepting commands"); else if(error ==3) Serial.println("Controller refusing to enter Pressures mode, may not support it."); // Check for the type of controller type =ps2x.readType(); switch(type) { case 0:Serial.println("Unknown Controller type"); pausa; case 1:Serial.println("DualShock Controller Found"); pausa; case 2:Serial.println("GuitarHero Controller Found"); pausa; }}// Main loopvoid loop(){ if(error ==1) //skip loop if no controller found return; else { //DualShock Controller ps2x.read_gamepad(false, vibrate); // disable vibration of the controller int nJoyX =ps2x.Analog(PSS_LX); // read x-joystick int nJoyY =ps2x.Analog(PSS_LY); // read y-joystick nJoyX =map(nJoyX, 0, 255, -1023, 1023); nJoyY =map(nJoyY, 0, 255, 1023, -1023); // OUTPUTS int nMotMixL; // Motor (left) mixed output int nMotMixR; // Motor (right) mixed output // CONFIG // - fPivYLimt :The threshold at which the pivot action starts // This threshold is measured in units on the Y-axis // away from the X-axis (Y=0). A greater value will assign // more of the joystick's range to pivot actions. // Allowable range:(0..+127) float fPivYLimit =1023.0; // TEMP VARIABLES float nMotPremixL; // Motor (left) premixed output float nMotPremixR; // Motor (right) premixed output int nPivSpeed; // Pivot Speed float fPivScale; // Balance scale between drive and pivot // Calculate Drive Turn output due to Joystick X input if (nJoyY>=0) { // Forward nMotPremixL =(nJoyX>=0)? 1023.0 :(1023.0 + nJoyX); nMotPremixR =(nJoyX>=0)? (1023.0 - nJoyX) :1023.0; } else { // Reverse nMotPremixL =(nJoyX>=0)? (1023.0 - nJoyX) :1023.0; nMotPremixR =(nJoyX>=0)? 1023.0 :(1023.0 + nJoyX); } // Scale Drive output due to Joystick Y input (throttle) nMotPremixL =nMotPremixL * nJoyY/1023.0; nMotPremixR =nMotPremixR * nJoyY/1023.0; // Now calculate pivot amount // - Strength of pivot (nPivSpeed) based on Joystick X input // - Blending of pivot vs drive (fPivScale) based on Joystick Y input nPivSpeed =nJoyX; fPivScale =(abs(nJoyY)>fPivYLimit)? 0.0 :(1.0 - abs(nJoyY)/fPivYLimit); // Calculate final mix of Drive and Pivot nMotMixL =(1.0-fPivScale)*nMotPremixL + fPivScale*( nPivSpeed); nMotMixR =(1.0-fPivScale)*nMotPremixR + fPivScale*(-nPivSpeed); motor_left_speed =nMotMixL; motor_right_speed =nMotMixR; if (motor_right_speed> 50) { digitalWrite(MOTORB_1,HIGH); digitalWrite(MOTORB_2,LOW); } else if (motor_right_speed <-50) { digitalWrite(MOTORB_1,LOW); digitalWrite(MOTORB_2, HIGH); } else { digitalWrite(MOTORB_1, LOW); digitalWrite(MOTORB_2, LOW); } if (motor_left_speed> 50) { digitalWrite(MOTORA_1, LOW); digitalWrite(MOTORA_2, HIGH); } else if (motor_left_speed <-50) { digitalWrite(MOTORA_1,HIGH); digitalWrite(MOTORA_2,LOW); } else { digitalWrite(MOTORA_1, LOW); digitalWrite(MOTORA_2, LOW); } analogWrite(ENA, abs(motor_left_speed)); analogWrite(ENB, abs(motor_right_speed)); if (abs(motor_left_speed> 50) || abs(motor_left_speed> 50)) { Serial.println("Moving!"); } delay(50); }} 

Esquemas