2022/7/23，转移代码用，勿看

#include <PS2X_lib.h>  //PS2的库文件，需先添加到Arduino IDE的库文件
#include <Servo.h>

/*PS2引脚定义*/
#define PS2_DAT_PIN        13//A3  //MOS
#define PS2_CMD_PIN        11//A2  //MIS
#define PS2_SEL_PIN        10//A4  //CS
#define PS2_CLK_PIN        12//A1  //SCK

/******************************************************************
 * 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  //振动模式

/*PS2设置*/
PS2X ps2x;                //定义ps2x为PS2X类变量
Servo myservo;            //定义伺服类变量

int error = 0;
byte type = 0;
byte vibrate = 0;
int turn=0;
/*小车运行状态枚举*/
enum {
  enSTOP = 1,
  enRUN,
  enBACK,
  enPower,
  enDown,
  enwait,
  enbreak,
  enleft,
  enright}
  enCarState;

/*电机引脚设置*/
int Left_motor_go = 8;    //左电机前进(AIN1)
int Left_motor_back = 7;  //左电机后退(AIN2)
int Left_motor_pwm = 6;   //左电机控速 PWMA
int power_on_off = 2;     //正转电机
int power_off_on = 3;

int servo_power = 4;     //伺服电机高电平
int servo_pwm = 5;       //伺服电机引脚

int servo_pos;//正反转
int servo_deg;//转速
int pos=90;
long int i=0,j=0;//测试
int turn_i=0,turn_j=0;
int servo_time=100; //脉冲时间
int plus_i=0,plus_j=0;
int pos_back=0;

/*小车初始速度控制*/
int CarSpeedControl = 150;

/*小车运动状态和舵机运动状态标志*/
int g_CarState = enSTOP;  //1前2后3左4右0停止
int g_power = enDown;
int g_servo_pos = enbreak;
/**
* Function       setup
* @author        Danny
* @date          2017.07.25
* @brief         初始化配置
* @param[in]     void
* @retval        void
* @par History   无
*/
void setup()
{
  //串口波特率设置
  Serial.begin(9600);

  //初始化电机驱动IO为输出方式
  pinMode(Left_motor_go, OUTPUT);
  pinMode(Left_motor_back, OUTPUT);
  pinMode(power_on_off, OUTPUT);
  pinMode(power_off_on, OUTPUT);
  pinMode(servo_power, OUTPUT);

  myservo.attach(servo_pwm);
  
  //在配置无线PS2模块之前，延时300ms以便于配置生效
  delay(300);

  /*PS2初始化*/
  error = ps2x.config_gamepad(PS2_CLK_PIN, PS2_CMD_PIN, PS2_SEL_PIN, PS2_DAT_PIN, pressures, rumble);
  if (error == 0)
  {
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
    if (pressures)
    {
      Serial.println("true ");
    }
    else
    {
      Serial.println("false");
    }
    Serial.print("rumble = ");
    if (rumble)
    {
      Serial.println("true)");
    }
    else
    {
      Serial.println("false");
    }
    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
    Serial.println("holding L1 or R1 will print out the analog stick values.");
    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
  }
  else if (error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

  else if (error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if (error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");

  //获取手柄类型
  type = ps2x.readType();
  switch (type) {
    case 0:
      Serial.print("Unknown Controller type found ");
      break;
    case 1:
      Serial.print("DualShock Controller found ");
      break;
    case 2:
      Serial.print("GuitarHero Controller found ");
      break;
    case 3:
      Serial.print("Wireless Sony DualShock Controller found ");
      break;
  }
}
void run()
{
  //左电机前进
  digitalWrite(Left_motor_go, HIGH);   //左电机前进使能
  digitalWrite(Left_motor_back, LOW);  //左电机后退禁止
  analogWrite(Left_motor_pwm, CarSpeedControl);
}
void brake()
{
  digitalWrite(Left_motor_go, LOW);
  digitalWrite(Left_motor_back, LOW);
}

void back()
{
  //左电机后退
  digitalWrite(Left_motor_go, LOW);     //左电机前进禁止
  digitalWrite(Left_motor_back, HIGH);  //左电机后退使能
  analogWrite(Left_motor_pwm, CarSpeedControl);
}

void power_on()
{
  digitalWrite(power_on_off, HIGH); //电机正转
  digitalWrite(power_off_on, HIGH);
}

void power_off()
{
  digitalWrite(power_on_off, LOW); //电机停转
  digitalWrite(power_off_on, LOW);
}

void servo_control(int g_servo_pos)//伺服电机控制
{
   digitalWrite(servo_power, HIGH); 

   switch(g_servo_pos)
   {
        case enleft:  servo_degree(0); delay(200);break;   //左转  
        case enright: servo_degree(180);delay(200);break;  //右转
        case enwait:  delay(200);break;                    //保持转角
        case enbreak: servo_degree(90);delay(200);break;   //复位
        default:      servo_degree(90);delay(200);break;   //防止意外
   }

             
}

//试验,参数都没定义

void servo_degree(int pos)
{
     if(pos>90&&turn_i<10)
     {
        digitalWrite(servo_pos,LOW);//一次发送25个脉冲（可修改），规定最小转角
        for(i=0;i<25;i++)
        {
         digitalWrite(servo_deg,HIGH);
         delayMicroseconds(servo_time);
         digitalWrite(servo_deg,LOW);
         delayMicroseconds(servo_time);
        }
        turn_i++;
        if(turn_i-turn_j>=0)
        {
          turn_i=turn_i-turn_j;
          turn_j=0;
        }

        Serial.print("turn_i = ");
        Serial.println(turn_i);
     }

     else if(pos<90&&turn_j<10)
     {
        digitalWrite(servo_pos,HIGH);
        for(j=0;j<25;j++)
        {
         digitalWrite(servo_deg,HIGH);
         delayMicroseconds(servo_time);
         digitalWrite(servo_deg,LOW);
         delayMicroseconds(servo_time);
        }
        turn_j++;
        if(turn_j-turn_i>=0)
        {
          turn_j=turn_j-turn_i;
          turn_i=0;
        }      

        Serial.print("turn_j = ");
        Serial.println(turn_j);
     }
   
     if(pos==90)
     {
        plus_i=25*turn_i;
        plus_j=25*turn_j;
        pos_back=plus_i-plus_j;//确定转角绝对位置
        
        if(pos_back>0)
        {
            while(pos_back>0)//复位
            {
               digitalWrite(servo_pos,HIGH);
        
               digitalWrite(servo_deg,HIGH);
               delayMicroseconds(servo_time);
               digitalWrite(servo_deg,LOW);
               delayMicroseconds(servo_time);
               pos_back--;
              
            }
            turn_i=0;//数据清理
            turn_j=0;
            pos_back=0;
            Serial.println("left turn back");   
        }
        if(pos_back<0)
        {
            while(pos_back<0)
            {
               digitalWrite(servo_pos,LOW);
        
               digitalWrite(servo_deg,HIGH);
               delayMicroseconds(servo_time);
               digitalWrite(servo_deg,LOW);
               delayMicroseconds(servo_time);
               pos_back++;
              
            }
            turn_i=0;
            turn_j=0;           
            pos_back=0;
            Serial.println("right turn back");   
        }
   
             
     }
     
}

void PS2_control(void)
{
  //摇杆方向值变量的定义
  int X1, Y1, X2, Y2;
  //如果手柄初始化失败或者手柄类型没找到,则返回
  if (error == 1)           //skip loop if no controller found
    return;
  if (type != 1)            //skip loop if no controller found
    return;

  //DualShock Controller
  ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed

  //手柄上的START键按下
  if (ps2x.Button(PSB_START))        //will be TRUE as long as button is pressed
    Serial.println("Start is being held");

  //手柄上的SELECT键按下
  if (ps2x.Button(PSB_SELECT))
    Serial.println("Select is being held");

  //手柄上的方向键上按下
  if (ps2x.Button(PSB_PAD_UP))      //will be TRUE as long as button is pressed
  {
    Serial.print("Up held this hard: ");
    Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
    g_CarState = enRUN;
  }  
  else if (ps2x.Button(PSB_PAD_DOWN))//手柄上的方向键下按下
  {
    Serial.print("DOWN held this hard: ");
    Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
    g_CarState = enBACK;
  }
  else
  {
    g_CarState = enSTOP;
  }

 //打算在这加一个左右键控制伺服电机的函数

 if(ps2x.Button(PSB_PAD_LEFT))
 {
    Serial.print("LEFT held this hard: ");
    Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
    g_servo_pos = enleft;
 }
 else if(ps2x.Button(PSB_PAD_RIGHT))
 {
    Serial.print("RIGHT held this hard: ");
    Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
    g_servo_pos = enright;
 }
else if(ps2x.ButtonPressed(PSB_TRIANGLE))
{
    g_servo_pos = enbreak;
} 
else
{
    g_servo_pos = enwait;
}

  //this will set the large motor vibrate speed based on how hard you press the blue (X) button
  vibrate = ps2x.Analog(PSAB_CROSS);

  //L3即左侧的摇杆按下小车停止
  if (ps2x.Button(PSB_L3))
  {
    g_CarState = enSTOP;
    Serial.println("L3 pressed");
  }
  
  //L2即手柄左侧前方下面的按键按下即小车加速
  if (ps2x.Button(PSB_L2))
  {
    Serial.println("L2 pressed");
    CarSpeedControl += 50;
    if (CarSpeedControl > 255)
    {
      CarSpeedControl = 255;
    }
  }
  
  //R2即手柄右侧前方下面的按键按下即小车减速
  if (ps2x.Button(PSB_R2))
  {
    Serial.println("R2 pressed");
    CarSpeedControl -= 50;
    if (CarSpeedControl < 50)
    {
      CarSpeedControl = 100;
    }
  }

  if (ps2x.ButtonPressed(PSB_CIRCLE))
  {
    //turn = 1;
    if(turn!=1)//按一下电机开，一下电机关
    {
       turn = 1;
       delay(100);//不延时可能会出现读取错误
    }
    else
    {
       turn =0;
       delay(100);
    }
  if(turn==0)
  {
     // Serial.println("Circle pressed:poweroff");
      g_power = enDown;
  }
  if(turn==1)
  {
     // Serial.println("Circle pressed:poweron");
      g_power = enPower;
  } 

  //当手柄左侧前方上面或右侧前方上面的按键按下时读取摇杆数据
  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);

    //读取摇杆的数据
    Y1 = ps2x.Analog(PSS_LY);
    X1 = ps2x.Analog(PSS_LX);
    Y2 = ps2x.Analog(PSS_RY);
    X2 = ps2x.Analog(PSS_RX);

    /*左摇杆控制小车运动状态程序*/
    if (Y1 < 5 && X1 > 80 && X1 < 180)         //上
    {
      g_CarState = enRUN;
    }
    else if (Y1 > 230 && X1 > 80 && X1 < 180) //下
    {
      g_CarState = enBACK;
    }
    else                                    //停
    {
      g_CarState = enSTOP;
    }

    //右摇杆控制伺服转向
    if (X2 < 5 && Y2 > 80 && Y2 < 180)         //左
    {
      g_servo_pos = enleft;
    }
    else if (X2 > 230 && Y2 > 80 && Y2 < 180) //右
    {
      g_servo_pos = enright;
    }
    else                                    //停
    {
      g_servo_pos = enbreak;
    }
   
  } 
   
    if (!ps2x.Button(PSB_L1)||!ps2x.Button(PSB_PAD_UP)||ps2x.Button(PSB_PAD_DOWN))
    {
        g_CarState = enSTOP;
    }
 /*   if (!ps2x.Button(PSB_R1))//松开键复位，视情况注释
    {
        g_servo_pos = enbreak;
    }
*/  
}

void loop()
{
  //状态标识
  int flag = 0;
  PS2_control();
  servo_control(g_servo_pos);
  switch (g_CarState)
  {
    case enSTOP: brake(); break;
    case enRUN: run();  break;
    case enBACK: back(); break;
    default: break;
  }

  switch(g_power)
  {
    case enPower: power_on(); Serial.println("Circle pressed:poweron");break;
    case enDown: power_off(); //Serial.println("Circle pressed:poweroff");break;
    default: break;
  }

 //下面的延时是必须要的,主要是为了避免过于频繁的发送手柄指令造成的不断重启
  delay(50);
}