Code - Basic 2 motor Mini Bot
This code can be Copy/Pasted directly into the Arduino IDE.
/* PS4 to ESP32
Must use the following configuration and setup instruction
Install Latest Version of Arduino (This was writen on version 2.0.3)
Add to preferences/Additional board manager ULRs :https://dl.espressif.com/dl/package_esp32_index.json
Add to Board manager "ESP32" by Espressif Systemsin, latest version
Download the PS4-esp32 Library zip file from: https://github.com/aed3/PS4-esp32/archive/refs/heads/master.zip
Add "PS4-esp32-master" zip file to Library Manager
Use a genuine PS4 controller, generic versions sometimes seem to have trouble
Download and install SixaxisPairTool software from: https://drive.google.com/file/d/1hlqxN8fK2DFNAky86s3Zt4i5X-Zo592s/view?usp=share_link
Use SixaxisPairTool software to find the Mac address save to the PS4 contoller
Enter the mac address from the controller into the section under setup().
For target board choose "ESP32 dev module" if ESP32 Dev kit is used.
Optional
if usb issue, try install CH340 Driver : https://learn.sparkfun.com/tutorials/how-to-install-ch340-drivers/all
Description:
This code connects to a specific MAC Address which is already stored on the PS4 Controller.
When connected the PS4 controller, left/right motion is mapped to right X axis and forward/reverse to left Y axis.
When not connected motion is stopped
*/
// Included library
#include <PS4Controller.h>
// IO Interface Definitions
#define StatusLED 2
#define ControllerSelect 4
#define motorPin_L_1 16
#define motorPin_L_2 17
#define motorPin_R_1 18
#define motorPin_R_2 19
// PWM Configuration Definitions
const int Motor_L_1 = 0;
const int Motor_L_2 = 1;
const int Motor_R_1 = 2;
const int Motor_R_2 = 3;
const int freq_Drive = 20000;
const int resolution = 8;
// Control Configurations
int DeadBand = 20;
int TurnRate = 1.25;
int MaxSpeed = 255;
int MinStartingSpeed = 160;
void setup() {
PS4.begin("01:02:03:04:05:06"); //ENTER YOUR CONTROLLER'S MAC ADDRESS HERE!
// confguring digital IO
pinMode(StatusLED, OUTPUT);
pinMode(motorPin_L_1, OUTPUT);
pinMode(motorPin_L_2, OUTPUT);
pinMode(motorPin_R_1, OUTPUT);
pinMode(motorPin_R_2, OUTPUT);
// output preset bias
digitalWrite(StatusLED, 0);
digitalWrite(motorPin_L_1, 0);
digitalWrite(motorPin_L_2, 0);
digitalWrite(motorPin_R_1, 0);
digitalWrite(motorPin_R_2, 0);
// configure LED PWM functionalities
ledcSetup(Motor_L_1, freq_Drive, resolution);
ledcSetup(Motor_L_2, freq_Drive, resolution);
ledcSetup(Motor_R_1, freq_Drive, resolution);
ledcSetup(Motor_R_2, freq_Drive, resolution);
// attach the channel to the GPIO to be controlled
ledcAttachPin(motorPin_L_1, Motor_L_1);
ledcAttachPin(motorPin_L_2, Motor_L_2);
ledcAttachPin(motorPin_R_1, Motor_R_1);
ledcAttachPin(motorPin_R_2, Motor_R_2);
}
void loop() {
if (PS4.isConnected()) {
digitalWrite(StatusLED, 1);
// store the joystick values
int Y_Axis = PS4.LStickY();
int X_Axis = PS4.RStickX();
// calutating arcade drive values for each motor
if (abs(Y_Axis) < DeadBand) Y_Axis = 0;
int L_value, R_value = 0;
if (abs(X_Axis) > DeadBand) {
L_value = Y_Axis - (TurnRate * X_Axis);
R_value = Y_Axis + (TurnRate * X_Axis);
}
else {
L_value = (Y_Axis);
R_value = (Y_Axis);
}
L_value = constrain(L_value, -127, 127);
R_value = constrain(R_value, -127, 127);
// this section of the code handles setting the motor PWM outputs for each motor
if (L_value > (DeadBand)) {
ledcWrite(Motor_L_1, map(abs(L_value), 0, MaxSpeed, MinStartingSpeed, MaxSpeed));
ledcWrite(Motor_L_2, 0);
} else if (L_value < (-DeadBand)) {
ledcWrite(Motor_L_1, 0);
ledcWrite(Motor_L_2, map(abs(L_value), 0, MaxSpeed, MinStartingSpeed, MaxSpeed));
} else {
ledcWrite(Motor_L_1, 0);
ledcWrite(Motor_L_2, 0);
}
if (R_value > (DeadBand)) {
ledcWrite(Motor_R_1, map(abs(R_value), 0, MaxSpeed, MinStartingSpeed, MaxSpeed));
ledcWrite(Motor_R_2, 0);
} else if (R_value < (-DeadBand)) {
ledcWrite(Motor_R_1, 0);
ledcWrite(Motor_R_2, map(abs(R_value), 0, MaxSpeed, MinStartingSpeed, MaxSpeed));
} else {
ledcWrite(Motor_R_1, 0);
ledcWrite(Motor_R_2, 0);
}
}
else {
// turn off status LED and stop motors
digitalWrite(StatusLED, 0);
ledcWrite(Motor_L_1, 0);
ledcWrite(Motor_L_2, 0);
ledcWrite(Motor_R_1, 0);
ledcWrite(Motor_R_2, 0);
}
}