“109010019 舵机转向小车套件”的版本间的差异

/*
  舵机转向小车PS2控制
 
  控制方式：左摇杆控制小车的前进后退（前后有效）,右摇杆控制方向（左右有效）；
  控制均为比例控制,遥感倾斜度越大,速度/角度越大;按键部分同理.
 
  手柄前端L1,L2调节舵机的居中角度位置,每次增加减小1度,右侧R1,R2调节电机速度.
 
  YFROBOT-WST
  WWW.YFROBOT.COM.CN
  09/19/2020
*/
 
/******************************************
   导入必须的库文件
 *****************************************/
#include <Servo.h>  //舵机库,软件自带
//#include "PS2X_lib.h"  //PS2手柄库,同程序文件夹
#include <PS2X_lib.h>  //PS2手柄库,同程序文件夹
 
PS2X ps2x;
Servo myservo;
 
/******************************************
   PS2手柄引脚设置端口，如需改动请保持软硬件同步
   电机驱动端口设置；舵机端口设置
 *****************************************/
#define PS2_DAT        A4
#define PS2_CMD        A0   //PS2 pin
#define PS2_SEL        A1
#define PS2_CLK        A2
 
#define PWM            5   //电机速度引脚
#define DIR            4   //电机方向引脚
#define SERVOPIN       3   //舵机引脚
 
/***************************************************
   默认居中90度情况下，最小值为50，最大值为130
 **************************************************/
#define SERVOMIN 55       //舵机左转最大值
#define SERVOMID 90       //舵机回中值
#define SERVOMAX 125      //舵机右转最大值
#define TURNLEFT myservo.write(SERVOMIN)   //左转
#define TURNMID myservo.write(SERVOMID)    //回中
#define TURNRIGHT myservo.write(SERVOMAX)  //右转
/*************************************************
   电机速度控制，按键控制速度值设置
 *************************************************/
#define MOTORFORWARD setMotor(100)   //按键前进速度
#define MOTORBACK setMotor(-100)    //按键后退速度
#define MOTORSTOP setMotor(0)       //停止
 
int MAXSPEED = 150;          //摇杆控制电机默认速度，可修改范围0-255.
int MIDSPEED = MAXSPEED;
int SERVOPOS = SERVOMID;
 
bool EnableRockerR = 1;
bool EnableRockerL = 1;
int RMid = 0 ;
int LStop = 0 ;
 
#define pressures   false    //按键模拟值关闭
#define rumble      true     //震动电机打开
int error = 0;
byte type = 0;
byte vibrate = 0;
 
void setup() {
  Serial.begin(57600);         // Baud rate
  pinMode(PWM, OUTPUT);        // Pin state
  pinMode(DIR, OUTPUT);
  myservo.attach(SERVOPIN);
  SERVOMID;                     // Servo middle
  delay(300);
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, 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");
  }
  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(* resetFunc) (void) = 0; //重新配对
 
void loop() {
  if (error == 1) //skip loop if no controller found
    resetFunc();
  ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
 
  if (ps2x.Button(PSB_START))        //will be TRUE as long as button is pressed
    Serial.println("Start is being held");
  if (ps2x.Button(PSB_SELECT))
    Serial.println("Select is being held");
 
  /*************************************************
     PS2手柄左边按键部分控制电机
   *************************************************/
  if (ps2x.ButtonPressed(PSB_PAD_UP)) {          // 按下前进
    //    Serial.println("PAD_UP just pressed");
    MOTORFORWARD;
    EnableRockerL = 0;
  } else if (ps2x.ButtonReleased(PSB_PAD_UP)) {   //松开停止
    //    Serial.println(" PAD_UP just released");
    MOTORSTOP;
    EnableRockerL = 1;
    LStop = 0;
  }
 
  if (ps2x.ButtonPressed(PSB_PAD_DOWN)) {         //按下后退
    //    Serial.println("PAD_DOWN just pressed");
    MOTORBACK;
    EnableRockerL = 0;
  } else if (ps2x.ButtonReleased(PSB_PAD_DOWN)) {  //松开停止
    //    Serial.println(" PAD_DOWN just released");
    MOTORSTOP;
    EnableRockerL = 1;
    LStop = 0;
  }
 
  /***********************************************************************
     Rocker program
     Through the rocker arm around before and after the change of
     resistance for motor speed control
     Up and Left（0 - 127）
     Down and Right （129 - 255）
     摇杆比例控制程序
        通过摇杆前后左右阻值的变化实现电机转速控制
        上左范围（0 - 127）
        下右范围（129 - 255）
  ***********************************************************************/
  if (EnableRockerL) {
    if (0 <= ps2x.Analog(PSS_LY) && ps2x.Analog(PSS_LY) <= 115) {
      digitalWrite(DIR, LOW);
      analogWrite(PWM, map(ps2x.Analog(PSS_LY), 0, 115, MAXSPEED, 0));
      LStop = 0;
    } else if (255 >= ps2x.Analog(PSS_LY) && ps2x.Analog(PSS_LY) >= 135) {
      digitalWrite(DIR, HIGH);
      analogWrite(PWM, map(ps2x.Analog(PSS_LY), 140, 255, 0, MAXSPEED));
      LStop = 0;
    } else {
      if (LStop == 0) {
        MOTORSTOP;
        LStop ++;
      }
    }
  }
 
  /*************************************************
     PS2手柄右边按键控制舵机代码
   *************************************************/
  if (ps2x.ButtonPressed(PSB_CIRCLE)) {             //按下右转
    //    Serial.println("Circle just pressed");         //will be TRUE if button was JUST pressed
    TURNRIGHT;
    EnableRockerR = 0;
  } else if (ps2x.ButtonReleased(PSB_CIRCLE)) {     //松开回中
    //    Serial.println("Circle just released");
    TURNMID;
    EnableRockerR = 1;
    RMid = 0;
  }
 
  if (ps2x.ButtonPressed(PSB_SQUARE)) {            //按下左转
    //    Serial.println("Square just pressed");
    TURNLEFT;
    EnableRockerR = 0;
  } else if (ps2x.ButtonReleased(PSB_SQUARE)) {    //松开回中
    //    Serial.println("Square just released");
    TURNMID;
    EnableRockerR = 1;
    RMid = 0;
  }
 
 
  if (EnableRockerR) {
    if (0 <= ps2x.Analog(PSS_RX) && ps2x.Analog(PSS_RX) <= 115) {
      myservo.write(map(ps2x.Analog(PSS_RX), 0, 115, SERVOMIN, SERVOPOS));
      RMid = 0;
    } else if (255 >= ps2x.Analog(PSS_RX) && ps2x.Analog(PSS_RX) >= 130) {
      myservo.write(map(ps2x.Analog(PSS_RX), 130, 255, SERVOPOS, SERVOMAX));
      RMid = 0;
    } else {
      if (RMid == 0) {
        TURNMID;
        RMid ++;
      }
    }
  }
 
  //电机速度增加20，最大增加到255（全速） 摇杆部分
  if (ps2x.ButtonPressed(PSB_R1)) {
    MAXSPEED += 20;
    if (MAXSPEED > 255) {
      MAXSPEED = 255;
    }
    vibrate = 100;
  }
 
  //电机速度减少20，最低到0（停止）
  if (ps2x.ButtonPressed(PSB_R2)) {
    MAXSPEED -= 20;
    if (MAXSPEED <= 0) {
      MAXSPEED = 0;
    }
    vibrate = 100;
  }
 
  //恢复电机初始速度值
  if (ps2x.ButtonPressed(PSB_CROSS)) {
    // reset speed;
    MAXSPEED = MIDSPEED;
    vibrate = 100;
  }
 
  if (ps2x.ButtonReleased(PSB_L1) || ps2x.ButtonReleased(PSB_L2) ||
      ps2x.ButtonReleased(PSB_R1) || ps2x.ButtonReleased(PSB_R2) || ps2x.ButtonReleased(PSB_CROSS)) {
    vibrate = 0;
    //    Serial.println("vibrate = 0");
  }
 
  delay(50);
}
 
//电机驱动函数，按键部分
void setMotor(int Speed) {
  if (Speed > 0) {
    digitalWrite(DIR, LOW);
    analogWrite(PWM, Speed);
  } else if (Speed < 0) {
    digitalWrite(DIR, HIGH);
    analogWrite(PWM, abs(Speed));
  } else {
    analogWrite(PWM, LOW);
    digitalWrite(DIR, LOW);
  }
}