智宇科技 ZYARJX-1机械臂智能小车 —— PS2游戏手柄控制程序

智宇科技 ZYARJX-1机械臂智能小车 —— PS2游戏手柄控制程序头文件#ifndefPS2X_lib_h#definePS2X_lib_h#ifARDUINO>22#include"Arduino.h"#else#inclu

头文件

#ifndef PS2X_lib_h
  #define PS2X_lib_h

#if ARDUINO > 22
  #include "Arduino.h"
#else
  #include "WProgram.h"
#endif

#include <math.h>
#include <stdio.h>
#include <stdint.h>
#ifdef __AVR__
  // AVR
  #include <avr/io.h>
  #define CTRL_CLK 4
  #define CTRL_BYTE_DELAY 3
#else
  // Pic32...
  #include <pins_arduino.h>
  #define CTRL_CLK 5
  #define CTRL_CLK_HIGH 5
  #define CTRL_BYTE_DELAY 4
#endif 

//These are our button constants
#define PSB_SELECT 0x0001
#define PSB_L3 0x0002
#define PSB_R3 0x0004
#define PSB_START 0x0008
#define PSB_PAD_UP 0x0010
#define PSB_PAD_RIGHT 0x0020
#define PSB_PAD_DOWN 0x0040
#define PSB_PAD_LEFT 0x0080
#define PSB_L2 0x0100
#define PSB_R2 0x0200
#define PSB_L1 0x0400
#define PSB_R1 0x0800
#define PSB_GREEN 0x1000
#define PSB_RED 0x2000
#define PSB_BLUE 0x4000
#define PSB_PINK 0x8000
#define PSB_TRIANGLE 0x1000
#define PSB_CIRCLE 0x2000
#define PSB_CROSS 0x4000
#define PSB_SQUARE 0x8000

//Guitar button constants
#define UP_STRUM 0x0010
#define DOWN_STRUM 0x0040
#define STAR_POWER 0x0100
#define GREEN_FRET 0x0200
#define YELLOW_FRET 0x1000
#define RED_FRET 0x2000
#define BLUE_FRET 0x4000
#define ORANGE_FRET 0x8000
#define WHAMMY_BAR 8

//These are stick values
#define PSS_RX 5
#define PSS_RY 6
#define PSS_LX 7
#define PSS_LY 8

//These are analog buttons
#define PSAB_PAD_RIGHT 9
#define PSAB_PAD_UP 11
#define PSAB_PAD_DOWN 12
#define PSAB_PAD_LEFT 10
#define PSAB_L2 19
#define PSAB_R2 20
#define PSAB_L1 17
#define PSAB_R1 18
#define PSAB_GREEN 13
#define PSAB_RED 14
#define PSAB_BLUE 15
#define PSAB_PINK 16
#define PSAB_TRIANGLE 13
#define PSAB_CIRCLE 14
#define PSAB_CROSS 15
#define PSAB_SQUARE 16

#define SET(x,y) (x|=(1<<y))
#define CLR(x,y) (x&=(~(1<<y)))
#define CHK(x,y) (x & (1<<y))
#define TOG(x,y) (x^=(1<<y))

class PS2X { 
   
  public:
    boolean Button(uint16_t);                //will be TRUE if button is being pressed
    unsigned int ButtonDataByte();
    boolean NewButtonState();
    boolean NewButtonState(unsigned int);    //will be TRUE if button was JUST pressed OR released
    boolean ButtonPressed(unsigned int);     //will be TRUE if button was JUST pressed
    boolean ButtonReleased(unsigned int);    //will be TRUE if button was JUST released
    void read_gamepad();
    boolean  read_gamepad(boolean, byte);
    byte readType();
    byte config_gamepad(uint8_t, uint8_t, uint8_t, uint8_t);
    byte config_gamepad(uint8_t, uint8_t, uint8_t, uint8_t, bool, bool);
    void enableRumble();
    bool enablePressures();
    byte Analog(byte);
    void reconfig_gamepad();

  private:
    inline void CLK_SET(void);
    inline void CLK_CLR(void);
    inline void CMD_SET(void);
    inline void CMD_CLR(void);
    inline void ATT_SET(void);
    inline void ATT_CLR(void);
    inline bool DAT_CHK(void);
    
    unsigned char _gamepad_shiftinout (char);
    unsigned char PS2data[21];
    void sendCommandString(byte*, byte);
    unsigned char i;
    unsigned int last_buttons;
    unsigned int buttons;
	
    #ifdef __AVR__
      uint8_t maskToBitNum(uint8_t);
      uint8_t _clk_mask; 
      volatile uint8_t *_clk_oreg;
      uint8_t _cmd_mask; 
      volatile uint8_t *_cmd_oreg;
      uint8_t _att_mask; 
      volatile uint8_t *_att_oreg;
      uint8_t _dat_mask; 
      volatile uint8_t *_dat_ireg;
    #else
      uint8_t maskToBitNum(uint8_t);
      uint16_t _clk_mask; 
      volatile uint32_t *_clk_lport_set;
      volatile uint32_t *_clk_lport_clr;
      uint16_t _cmd_mask; 
      volatile uint32_t *_cmd_lport_set;
      volatile uint32_t *_cmd_lport_clr;
      uint16_t _att_mask; 
      volatile uint32_t *_att_lport_set;
      volatile uint32_t *_att_lport_clr;
      uint16_t _dat_mask; 
      volatile uint32_t *_dat_lport;
    #endif
	
    unsigned long last_read;
    byte read_delay;
    byte controller_type;
    boolean en_Rumble;
    boolean en_Pressures;
};

#endif

C文件

#include "PS2X_lib.h"
#include <math.h>
#include <stdio.h>
#include <stdint.h>
#include <avr/io.h>
#if ARDUINO > 22
  #include "Arduino.h"
#else
  #include "WProgram.h"
  #include "pins_arduino.h"
#endif

static byte enter_config[]={ 
   0x01,0x43,0x00,0x01,0x00};
static byte set_mode[]={ 
   0x01,0x44,0x00,0x01,0x03,0x00,0x00,0x00,0x00};
static byte set_bytes_large[]={ 
   0x01,0x4F,0x00,0xFF,0xFF,0x03,0x00,0x00,0x00};
static byte exit_config[]={ 
   0x01,0x43,0x00,0x00,0x5A,0x5A,0x5A,0x5A,0x5A};
static byte enable_rumble[]={ 
   0x01,0x4D,0x00,0x00,0x01};
static byte type_read[]={ 
   0x01,0x45,0x00,0x5A,0x5A,0x5A,0x5A,0x5A,0x5A};

/****************************************************************************************/
boolean PS2X::NewButtonState() { 
   
  return ((last_buttons ^ buttons) > 0);
}

/****************************************************************************************/
boolean PS2X::NewButtonState(unsigned int button) { 
   
  return (((last_buttons ^ buttons) & button) > 0);
}

/****************************************************************************************/
boolean PS2X::ButtonPressed(unsigned int button) { 
   
  return(NewButtonState(button) & Button(button));
}

/****************************************************************************************/
boolean PS2X::ButtonReleased(unsigned int button) { 
   
  return((NewButtonState(button)) & ((~last_buttons & button) > 0));
}

/****************************************************************************************/
boolean PS2X::Button(uint16_t button) { 
   
  return ((~buttons & button) > 0);
}

/****************************************************************************************/
unsigned int PS2X::ButtonDataByte() { 
   
   return (~buttons);
}

/****************************************************************************************/
byte PS2X::Analog(byte button) { 
   
   return PS2data[button];
}

/****************************************************************************************/
unsigned char PS2X::_gamepad_shiftinout (char byte) { 
   
   unsigned char tmp = 0;
   for(unsigned char i=0;i<8;i++) { 
   
      if(CHK(byte,i)) CMD_SET();
      else CMD_CLR();
	  
      CLK_CLR();
      delayMicroseconds(CTRL_CLK);

      //if(DAT_CHK()) SET(tmp,i);
      if(DAT_CHK()) bitSet(tmp,i);

      CLK_SET();
#if CTRL_CLK_HIGH
      delayMicroseconds(CTRL_CLK_HIGH);
#endif
   }
   CMD_SET();
   delayMicroseconds(CTRL_BYTE_DELAY);
   return tmp;
}

/****************************************************************************************/
void PS2X::read_gamepad() { 
   
   read_gamepad(false, 0x00);
}

/****************************************************************************************/
boolean PS2X::read_gamepad(boolean motor1, byte motor2) { 
   
   double temp = millis() - last_read;

   if (temp > 1500) //waited to long
      reconfig_gamepad();

   if(temp < read_delay)  //waited too short
      delay(read_delay - temp);

   if(motor2 != 0x00)
      motor2 = map(motor2,0,255,0x40,0xFF); //noting below 40 will make it spin

   char dword[9] = { 
   0x01,0x42,0,motor1,motor2,0,0,0,0};
   byte dword2[12] = { 
   0,0,0,0,0,0,0,0,0,0,0,0};

   // Try a few times to get valid data...
   for (byte RetryCnt = 0; RetryCnt < 5; RetryCnt++) { 
   
      CMD_SET();
      CLK_SET();
      ATT_CLR(); // low enable joystick

      delayMicroseconds(CTRL_BYTE_DELAY);
      //Send the command to send button and joystick data;
      for (int i = 0; i<9; i++) { 
   
         PS2data[i] = _gamepad_shiftinout(dword[i]);
      }

      if(PS2data[1] == 0x79) { 
     //if controller is in full data return mode, get the rest of data
         for (int i = 0; i<12; i++) { 
   
            PS2data[i+9] = _gamepad_shiftinout(dword2[i]);
         }
      }

      ATT_SET(); // HI disable joystick
      // Check to see if we received valid data or not. 
	  // We should be in analog mode for our data to be valid (analog == 0x7_)
      if ((PS2data[1] & 0xf0) == 0x70)
         break;

      // If we got to here, we are not in analog mode, try to recover...
      reconfig_gamepad(); // try to get back into Analog mode.
      delay(read_delay);
   }

   // If we get here and still not in analog mode (=0x7_), try increasing the read_delay...
   if ((PS2data[1] & 0xf0) != 0x70) { 
   
      if (read_delay < 10)
         read_delay++;   // see if this helps out...
   }

#ifdef PS2X_COM_DEBUG
   Serial.println("OUT:IN");
   for(int i=0; i<9; i++){ 
   
      Serial.print(dword[i], HEX);
      Serial.print(":");
      Serial.print(PS2data[i], HEX);
      Serial.print(" ");
   }
   for (int i = 0; i<12; i++) { 
   
      Serial.print(dword2[i], HEX);
      Serial.print(":");
      Serial.print(PS2data[i+9], HEX);
      Serial.print(" ");
   }
   Serial.println("");
#endif

   last_buttons = buttons; //store the previous buttons states

#if defined(__AVR__)
   buttons = *(uint16_t*)(PS2data+3);   //store as one value for multiple functions
#else
   buttons =  (uint16_t)(PS2data[4] << 8) + PS2data[3];   //store as one value for multiple functions
#endif
   last_read = millis();
   return ((PS2data[1] & 0xf0) == 0x70);  // 1 = OK = analog mode - 0 = NOK
}

/****************************************************************************************/
byte PS2X::config_gamepad(uint8_t clk, uint8_t cmd, uint8_t att, uint8_t dat) { 
   
   return config_gamepad(clk, cmd, att, dat, false, false);
}

/****************************************************************************************/
byte PS2X::config_gamepad(uint8_t clk, uint8_t cmd, uint8_t att, uint8_t dat, bool pressures, bool rumble) { 
   

  byte temp[sizeof(type_read)];

#ifdef __AVR__
  _clk_mask = digitalPinToBitMask(clk);
  _clk_oreg = portOutputRegister(digitalPinToPort(clk));
  _cmd_mask = digitalPinToBitMask(cmd);
  _cmd_oreg = portOutputRegister(digitalPinToPort(cmd));
  _att_mask = digitalPinToBitMask(att);
  _att_oreg = portOutputRegister(digitalPinToPort(att));
  _dat_mask = digitalPinToBitMask(dat);
  _dat_ireg = portInputRegister(digitalPinToPort(dat));
#else
  uint32_t            lport;                   // Port number for this pin
  _clk_mask = digitalPinToBitMask(clk);
  lport = digitalPinToPort(clk);
  _clk_lport_set = portOutputRegister(lport) + 2;
  _clk_lport_clr = portOutputRegister(lport) + 1;

  _cmd_mask = digitalPinToBitMask(cmd);
  lport = digitalPinToPort(cmd);
  _cmd_lport_set = portOutputRegister(lport) + 2;
  _cmd_lport_clr = portOutputRegister(lport) + 1;

  _att_mask = digitalPinToBitMask(att);
  lport = digitalPinToPort(att);
  _att_lport_set = portOutputRegister(lport) + 2;
  _att_lport_clr = portOutputRegister(lport) + 1;

  _dat_mask = digitalPinToBitMask(dat);
  _dat_lport = portInputRegister(digitalPinToPort(dat));
#endif

  pinMode(clk, OUTPUT); //configure ports
  pinMode(att, OUTPUT);
  pinMode(cmd, OUTPUT);
  pinMode(dat, INPUT);

#if defined(__AVR__)
  digitalWrite(dat, HIGH); //enable pull-up
#endif

  CMD_SET(); // SET(*_cmd_oreg,_cmd_mask);
  CLK_SET();

  //new error checking. First, read gamepad a few times to see if it's talking
  read_gamepad();
  read_gamepad();

  //see if it talked - see if mode came back. 
  //If still anything but 41, 73 or 79, then it's not talking
  if(PS2data[1] != 0x41 && PS2data[1] != 0x73 && PS2data[1] != 0x79){ 
    
#ifdef PS2X_DEBUG
    Serial.println("Controller mode not matched or no controller found");
    Serial.print("Expected 0x41, 0x73 or 0x79, but got ");
    Serial.println(PS2data[1], HEX);
#endif
    return 1; //return error code 1
  }

  //try setting mode, increasing delays if need be.
  read_delay = 1;

  for(int y = 0; y <= 10; y++) { 
   
    sendCommandString(enter_config, sizeof(enter_config)); //start config run

    //read type
    delayMicroseconds(CTRL_BYTE_DELAY);

    CMD_SET();
    CLK_SET();
    ATT_CLR(); // low enable joystick

    delayMicroseconds(CTRL_BYTE_DELAY);

    for (int i = 0; i<9; i++) { 
   
      temp[i] = _gamepad_shiftinout(type_read[i]);
    }

    ATT_SET(); // HI disable joystick

    controller_type = temp[3];

    sendCommandString(set_mode, sizeof(set_mode));
    if(rumble){ 
    sendCommandString(enable_rumble, sizeof(enable_rumble)); en_Rumble = true; }
    if(pressures){ 
    sendCommandString(set_bytes_large, sizeof(set_bytes_large)); en_Pressures = true; }
    sendCommandString(exit_config, sizeof(exit_config));

    read_gamepad();

    if(pressures){ 
   
      if(PS2data[1] == 0x79)
        break;
      if(PS2data[1] == 0x73)
        return 3;
    }

    if(PS2data[1] == 0x73)
      break;

    if(y == 10){ 
   
#ifdef PS2X_DEBUG
      Serial.println("Controller not accepting commands");
      Serial.print("mode stil set at");
      Serial.println(PS2data[1], HEX);
#endif
      return 2; //exit function with error
    }
    read_delay += 1; //add 1ms to read_delay
  }
  return 0; //no error if here
}

/****************************************************************************************/
void PS2X::sendCommandString(byte string[], byte len) { 
   
#ifdef PS2X_COM_DEBUG
  byte temp[len];
  ATT_CLR(); // low enable joystick
  delayMicroseconds(CTRL_BYTE_DELAY);

  for (int y=0; y < len; y++)
    temp[y] = _gamepad_shiftinout(string[y]);

  ATT_SET(); //high disable joystick
  delay(read_delay); //wait a few

  Serial.println("OUT:IN Configure");
  for(int i=0; i<len; i++) { 
   
    Serial.print(string[i], HEX);
    Serial.print(":");
    Serial.print(temp[i], HEX);
    Serial.print(" ");
  }
  Serial.println("");
#else
  ATT_CLR(); // low enable joystick
  for (int y=0; y < len; y++)
    _gamepad_shiftinout(string[y]);
  ATT_SET(); //high disable joystick
  delay(read_delay);                  //wait a few
#endif
}

/****************************************************************************************/
byte PS2X::readType() { 
   
/* byte temp[sizeof(type_read)]; sendCommandString(enter_config, sizeof(enter_config)); delayMicroseconds(CTRL_BYTE_DELAY); CMD_SET(); CLK_SET(); ATT_CLR(); // low enable joystick delayMicroseconds(CTRL_BYTE_DELAY); for (int i = 0; i<9; i++) { temp[i] = _gamepad_shiftinout(type_read[i]); } sendCommandString(exit_config, sizeof(exit_config)); if(temp[3] == 0x03) return 1; else if(temp[3] == 0x01) return 2; return 0; */

  if(controller_type == 0x03)
    return 1;
  else if(controller_type == 0x01)
    return 2;
  else if(controller_type == 0x0C)  
    return 3;  //2.4G Wireless Dual Shock PS2 Game Controller
	
  return 0;
}

/****************************************************************************************/
void PS2X::enableRumble() { 
   
  sendCommandString(enter_config, sizeof(enter_config));
  sendCommandString(enable_rumble, sizeof(enable_rumble));
  sendCommandString(exit_config, sizeof(exit_config));
  en_Rumble = true;
}

/****************************************************************************************/
bool PS2X::enablePressures() { 
   
  sendCommandString(enter_config, sizeof(enter_config));
  sendCommandString(set_bytes_large, sizeof(set_bytes_large));
  sendCommandString(exit_config, sizeof(exit_config));

  read_gamepad();
  read_gamepad();

  if(PS2data[1] != 0x79)
    return false;

  en_Pressures = true;
    return true;
}

/****************************************************************************************/
void PS2X::reconfig_gamepad(){ 
   
  sendCommandString(enter_config, sizeof(enter_config));
  sendCommandString(set_mode, sizeof(set_mode));
  if (en_Rumble)
    sendCommandString(enable_rumble, sizeof(enable_rumble));
  if (en_Pressures)
    sendCommandString(set_bytes_large, sizeof(set_bytes_large));
  sendCommandString(exit_config, sizeof(exit_config));
}

/****************************************************************************************/
#ifdef __AVR__
inline void  PS2X::CLK_SET(void) { 
   
  register uint8_t old_sreg = SREG;
  cli();
  *_clk_oreg |= _clk_mask;
  SREG = old_sreg;
}

inline void  PS2X::CLK_CLR(void) { 
   
  register uint8_t old_sreg = SREG;
  cli();
  *_clk_oreg &= ~_clk_mask;
  SREG = old_sreg;
}

inline void  PS2X::CMD_SET(void) { 
   
  register uint8_t old_sreg = SREG;
  cli();
  *_cmd_oreg |= _cmd_mask; // SET(*_cmd_oreg,_cmd_mask);
  SREG = old_sreg;
}

inline void  PS2X::CMD_CLR(void) { 
   
  register uint8_t old_sreg = SREG;
  cli();
  *_cmd_oreg &= ~_cmd_mask; // SET(*_cmd_oreg,_cmd_mask);
  SREG = old_sreg;
}

inline void  PS2X::ATT_SET(void) { 
   
  register uint8_t old_sreg = SREG;
  cli();
  *_att_oreg |= _att_mask ;
  SREG = old_sreg;
}

inline void PS2X::ATT_CLR(void) { 
   
  register uint8_t old_sreg = SREG;
  cli();
  *_att_oreg &= ~_att_mask;
  SREG = old_sreg;
}

inline bool PS2X::DAT_CHK(void) { 
   
  return (*_dat_ireg & _dat_mask) ? true : false;
}

#else
// On pic32, use the set/clr registers to make them atomic...
inline void  PS2X::CLK_SET(void) { 
   
  *_clk_lport_set |= _clk_mask;
}

inline void  PS2X::CLK_CLR(void) { 
   
  *_clk_lport_clr |= _clk_mask;
}

inline void  PS2X::CMD_SET(void) { 
   
  *_cmd_lport_set |= _cmd_mask;
}

inline void  PS2X::CMD_CLR(void) { 
   
  *_cmd_lport_clr |= _cmd_mask;
}

inline void  PS2X::ATT_SET(void) { 
   
  *_att_lport_set |= _att_mask;
}

inline void PS2X::ATT_CLR(void) { 
   
  *_att_lport_clr |= _att_mask;
}

inline bool PS2X::DAT_CHK(void) { 
   
  return (*_dat_lport & _dat_mask) ? true : false;
}

#endif

arduino文件

#include "PS2X_lib.h" //for v1.6
#include <Wire.h>
#include <Servo.h>
/****************************************************************** * set pins connected to PS2 controller: * - 1e column: original * - 2e colmun: Stef? * replace pin numbers by the ones you use ******************************************************************/
#define PS2_DAT 6 
#define PS2_CMD 5 
#define PS2_SEL 4 
#define PS2_CLK 3 

/****************************************************************** * select modes of PS2 controller: * - pressures = analog reading of push-butttons * - rumble = motor rumbling * uncomment 1 of the lines for each mode selection ******************************************************************/
#define pressures true 
#define rumble true

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 connect the controller, 
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;
byte type = 0;
byte vibrate = 0;

int lx = 0;
int ly=  0;
int L_Speed = 0;
int R_Speed = 0;

Servo myservo1,myservo2,myservo3,myservo4;  // create servo object to control a servo
const int SERVOS = 4;  //舵机数4个

const int ACC = 10; // the accurancy of the potentiometer value before idle starts counting
int  value[SERVOS], idle[SERVOS], currentAngle[SERVOS], MIN[SERVOS], MAX[SERVOS], INITANGLE[SERVOS], previousAngle[SERVOS],ANA[SERVOS];

int Left_motor=8;     //左电机(IN3) 输出0 前进 输出1 后退
int Left_motor_pwm=9;     //左电机PWM调速

int Right_motor_pwm=10;    // 右电机PWM调速
int Right_motor=11;    // 右电机后退(IN1) 输出0 前进 输出1 后退


void setup(){ 
   
  Serial.begin(57600);
  
   myservo1.attach(2);  //手爪电机
   myservo2.attach(7);  //上臂电机
   myservo3.attach(12); //下臂电机
   myservo4.attach(13); //底座电机 
   
  //-----电机IO口定-
  pinMode( 8, OUTPUT);
  pinMode( 9, OUTPUT);
  pinMode( 10, OUTPUT);
  pinMode( 11, OUTPUT);
  stop();   
  //手爪 Servo
  MIN[0] = 10;
  MAX[0] = 50;
 INITANGLE[0] = 30;

  
  //上臂电机
  MIN[1] = 10; // This should bring the lever to just below 90deg to ground
  MAX[1] = 140;
  INITANGLE[1] = 90; // This should bring the lever parallel with the ground

  
  //下臂电机
  MIN[2] = 40;
  MAX[2] = 170;
  INITANGLE[2] = 90;

  
  //底座电机
  MIN[3] = 0;
  MAX[3] = 170;
  INITANGLE[3] = 90;
 
  //初始化电机
  myservo1.write(INITANGLE[0]);  
  myservo2.write(INITANGLE[1]);  
  myservo3.write(INITANGLE[2]);
  myservo4.write(INITANGLE[3]);  

   currentAngle[0]=INITANGLE[0];
   currentAngle[1]=INITANGLE[1];
   currentAngle[2]=INITANGLE[2];
   currentAngle[3]=INITANGLE[3];
   
  delay(2000);  //added delay to give wireless ps2 module some time to startup, before configuring it
  Serial.print("Search Controller..");
  //CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************
  do{ 
   
  //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
    error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
    if(error == 0){ 
   
      Serial.println("\nConfigured successful ");
      break;
    }else{ 
   
      Serial.print(".");
      delay(100);
    }
  }while(1);  
  type = ps2x.readType(); 
  switch(type) { 
   
    case 0:
      Serial.println("Unknown Controller type found ");
      break;
    case 1:
      Serial.println("DualShock Controller found ");
      break;
    case 2:
      Serial.println("GuitarHero Controller found ");
      break;
    case 3:
      Serial.println("Wireless Sony DualShock Controller found ");
      break;
   }
  //震动一,说明连接
   ps2x.read_gamepad(true, 200);  //开启震动
   delay(500);
   ps2x.read_gamepad(false, 200); //开启震动
   delay(300);
   ps2x.read_gamepad(true, 200);  //开启震动
   delay(500);
}

void loop() { 
   
  /* You must Read Gamepad to get new values and set vibration values ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255) if you don't enable the rumble, use ps2x.read_gamepad(); with no values You should call this at least once a second */  
    ps2x.read_gamepad(false, vibrate);  //开启震动
    
    if(ps2x.Button(PSB_START)){ 
            //开始按键
      Serial.println("Start is being held");
      ps2x.read_gamepad(true, 200);
    }
    if(ps2x.Button(PSB_SELECT))
      Serial.println("Select is being held");      
    if(ps2x.Button(PSB_PAD_UP))  //方向按键向上按下
    { 
                    
      Serial.print("Up held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
      forward();
       
    }else if(ps2x.ButtonReleased(PSB_PAD_UP))  { 
      //方向按键向上释放
        stop();      
    }
    if(ps2x.Button(PSB_PAD_DOWN))   //方向按键向下按下
    { 
               
      Serial.print("DOWN held this hard: ");
      Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
      back();
    }
    else if(ps2x.ButtonReleased(PSB_PAD_DOWN))//方向按键向下释放
    { 
     
        stop();  
    }
    if(ps2x.Button(PSB_R1)){ 
                             //侧面开关右R1
      R_Speed = 0;                 
    }
    if(ps2x.Button(PSB_L1)){ 
                             //侧面开关左L1
      L_Speed = 0;               
    } 
    if(ps2x.Button(PSB_PAD_LEFT)) //方向按键左侧按下 
    { 
                             
      left();
    }else if(ps2x.ButtonReleased(PSB_PAD_LEFT))  //方向按键左侧释放
    { 
       
       stop();
    }
    if(ps2x.Button(PSB_PAD_RIGHT))   //方向按键右侧按下
    { 
                      
      Serial.println(ps2x.Analog(PSB_PAD_RIGHT));
        right();
    }
    else if(ps2x.ButtonReleased(PSB_PAD_RIGHT))  //方向按键右侧释放
    { 
      
// Serial.println("*****PSB_PAD_RIGHT***** ");
      stop();     
    }   
    vibrate = ps2x.Analog(PSAB_CROSS);  
    //this will set the large motor vibrate speed based on how hard you press the blue (X) button
    { 
   
      
    }
  
    if (ps2x.NewButtonState()) { 
           //will be TRUE if any button changes state (on to off, or off to on)
      if(ps2x.Button(PSB_L3))         //左侧L3
        Serial.println("L3 pressed");
      if(ps2x.Button(PSB_R3))         //右侧L3
        Serial.println("R3 pressed");
      if(ps2x.Button(PSB_L2))         //左侧L2
        Serial.println("L2 pressed");
      if(ps2x.Button(PSB_R2))         //右侧R2
        Serial.println("R2 pressed");
      if(ps2x.Button(PSB_TRIANGLE))
        Serial.println("Triangle pressed");   //三角形按键按下 
    }
    if(ps2x.ButtonPressed(PSB_CIRCLE))
    { 
                  
      //will be TRUE if button was JUST pressed
      Serial.println("Circle just pressed");
      openGripper(); 
    }
    if(ps2x.NewButtonState(PSB_CROSS)) { 
         // ×键新键改变
      Serial.println("X just changed");
      ps2x.read_gamepad(true, vibrate);       //获取震动值
     }
    if(ps2x.ButtonPressed(PSB_SQUARE)) 
    { 
                
      //will be TRUE if button was JUST released
      Serial.println("Square just released");
      closeGripper() ;
    }
    if(ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1)) { 
    //print stick values if either is TRUE
      Serial.print("Stick Values:");
      Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX 
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_LX), DEC); 
      Serial.print(",");
      Serial.print(ps2x.Analog(PSS_RY), DEC); 
      Serial.print(",");
      Serial.println(ps2x.Analog(PSS_RX), DEC);      
    }     
 
    value[0] = ps2x.Analog(PSS_LX);
    value[1] = ps2x.Analog(PSS_RY);
    value[2] = ps2x.Analog(PSS_LY);
    value[3] = ps2x.Analog(PSS_RX);
  
   for (int i = 0; i < SERVOS; i++)
   { 
   
      if (value[i] > 130) 
      { 
   
        if (currentAngle[i] < MAX[i]) 
        currentAngle[i]+=1;
// Serial.print("value"); 
// Serial.print(i); 
// Serial.print(":"); 
// Serial.println(value[i]);
          switch(i)
          { 
   
              
            case 0:  myservo1.write(currentAngle[i]);break;
            case 1:  myservo2.write(currentAngle[i]);break;
            case 2:  myservo3.write(currentAngle[i]);break;
            case 3:  myservo4.write(currentAngle[i]);break;
          }   
      } 
    else if (value[i] < 120) 
    { 
   
      if (currentAngle[i] > MIN[i]) currentAngle[i]-=1;
// Serial.print("value"); 
// Serial.print(i); 
// Serial.print(":"); 
// Serial.println(value[i]);
       switch(i)
      { 
   
       case 0:  myservo1.write(currentAngle[i]);break;
       case 1:  myservo2.write(currentAngle[i]);break;
       case 2:  myservo3.write(currentAngle[i]);break;
       case 3:  myservo4.write(currentAngle[i]);break;
      }   
    }  
  }  
  delay(10);
}

//Grab something
void openGripper() { 
   
  myservo1.write(MIN[0]);
  delay(300);
}

//Let go of something
void closeGripper() { 
   
   myservo1.write(MAX[0]);
  delay(300);
}

void forward()
{ 
   
  digitalWrite(8, LOW);
  digitalWrite(9, HIGH);
   
  digitalWrite(11, LOW); 
  digitalWrite(10, HIGH);
}

void right()
{ 
   
   digitalWrite(8, LOW);
  digitalWrite(9, HIGH);
  
  digitalWrite(11, LOW);
  digitalWrite(10, LOW);
}

void back()
{ 
   
  digitalWrite(8, HIGH);
  digitalWrite(9, HIGH);
  
  digitalWrite(11, HIGH);
  digitalWrite(10, HIGH);
}

void left()
{ 
   
  digitalWrite(8, LOW);
  digitalWrite(9, LOW);
  
  digitalWrite(11, LOW);
  digitalWrite(10, HIGH);
}

void stop()
{ 
   
  digitalWrite(8, LOW);
  digitalWrite(9, LOW);
  
  digitalWrite(11, LOW);
  digitalWrite(10, LOW); 
}

今天的文章智宇科技 ZYARJX-1机械臂智能小车 —— PS2游戏手柄控制程序分享到此就结束了,感谢您的阅读。

版权声明:本文内容由互联网用户自发贡献,该文观点仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 举报,一经查实,本站将立刻删除。
如需转载请保留出处:https://bianchenghao.cn/62220.html

(0)
编程小号编程小号

相关推荐

发表回复

您的电子邮箱地址不会被公开。 必填项已用*标注