I am connecting a JHEMCU SPP-SBUS PPM PWM signal conversion module to an Arduino UNO Tx as input (SBUS) and 6 motors connected to the PWM 1-6 pins. The goal is to use a PS2 controller to maneuver an underwater robot. However, I am having problems with the communication protocols because they present a delay in the movements and sometimes activate motors that are not the desired ones.
I can check that the mapping and PS2 controller sections are correct in the code.
/*
The purpose is to connect 6 motors to a signal converter (JHEMCU SPP-SBUS PPM PWM signal conversion module Interchanger RC remote control receiver)
and this to an arduino. The directions will be controlled with a PS2 controller connected to the arduino.
*/
#include <PS2X_lib.h>
#include <SoftwareSerial.h>
#define SBUS_SERIAL_RX 10 // RX pin of SoftwareSerial
#define SBUS_SERIAL_TX 11 // TX pin of SoftwareSerial
SoftwareSerial SBUS_SERIAL(SBUS_SERIAL_RX, SBUS_SERIAL_TX);
//SoftwareSerial SBUS_SERIAL(SBUS_SERIAL_RX, SBUS_SERIAL_TX);
PS2X ps2x; // create PS2 Controller Class
// Function declarations
void sendPWM(int channel, int value);
void processSBUSData();
void controlMotors(int leftX, int leftY, int rightX, int rightY);
//global variables
int prevLeftJoyX = 0, prevLeftJoyY = 0, prevRightJoyX = 0, prevRightJoyY = 0;
bool prevL1 = false, prevL2 = false, prevR1 = false, prevR2 = false;
void setup() {
Serial.begin(115200);
SBUS_SERIAL.begin(100000); // Set the baud rate to 100000 and the data format to 8N2 for SBUS
// Setup the PS2 controller
ps2x.config_gamepad(13, 11, 10, 12, true, true);
//Serial.println("PS2 Controller Check");
if (ps2x.readType() == 1) {
//Serial.println("PS2 Controller Found");
} else {
//Serial.println("No PS2 Controller Found");
while (1); // halt the program
}
}
void loop() {
// Read PS2 controller input
ps2x.read_gamepad();
// Create variables for reading the joysticks
int leftJoyX = ps2x.Analog(PSS_LX);
int leftJoyY = ps2x.Analog(PSS_LY);
int rightJoyX = ps2x.Analog(PSS_RX);
int rightJoyY = ps2x.Analog(PSS_RY);
// Check if the controller is connected and if the joysticks or buttons have changed
if (!ps2x.Button(PSB_SELECT) &&
(leftJoyX != prevLeftJoyX || leftJoyY != prevLeftJoyY ||
rightJoyX != prevRightJoyX || rightJoyY != prevRightJoyY ||
ps2x.Button(PSB_L1) != prevL1 || ps2x.Button(PSB_L2) != prevL2 ||
ps2x.Button(PSB_R1) != prevR1 || ps2x.Button(PSB_R2) != prevR2)) {
// Print current values when joysticks are moved or buttons state changed
Serial.print("Left Joystick X: ");
Serial.print(leftJoyX);
Serial.print(" Y: ");
Serial.println(leftJoyY);
Serial.print("Right Joystick X: ");
Serial.print(rightJoyX);
Serial.print(" Y: ");
Serial.println(rightJoyY);
// Call the motor control function with joystick positions
controlMotors(leftJoyX, leftJoyY, rightJoyX, rightJoyY);
// Update previous states
prevLeftJoyX = leftJoyX;
prevLeftJoyY = leftJoyY;
prevRightJoyX = rightJoyX;
prevRightJoyY = rightJoyY;
prevL1 = ps2x.Button(PSB_L1);
prevL2 = ps2x.Button(PSB_L2);
prevR1 = ps2x.Button(PSB_R1);
prevR2 = ps2x.Button(PSB_R2);
}
delay(50); // Add a delay to match the SBUS frame rate of approximately 50 Hz
// Process SBUS data
//processSBUSData();
}
void controlMotors(int leftX, int leftY, int rightX, int rightY) {
// Define the maximum speed value
const int maxSpeed = 255;
// Map joystick values to motor speeds
int motor1Speed = map(leftY, 0, 255, 172, 1811);
int motor3Speed = map(leftY, 0, 255, 172, 1811);
int motor2Speed = map(rightY, 0, 255, 172, 1811);
int motor4Speed = map(rightY, 0, 255, 172, 1811);
int motor5Speed = 0;
int motor6Speed = 0;
// Implement motor control logic based on joystick positions
if (leftY >= 0 && leftY <128 ) {
// N: motor1 fwd, motor3 fwd
motor1Speed = abs(motor1Speed);
motor3Speed = abs(motor3Speed);
} else if (leftY > 128) {
// S: motor1 rvs, motor3 rvs
motor1Speed = -abs(motor1Speed);
motor3Speed = -abs(motor3Speed);
} else {
// Stop motors if joystick is in the center
//motor1Speed = 0;
//motor3Speed = 0;
}
if (rightY >= 0 && rightY < 128) {
// N: motor2 fwd, motor4 fwd
motor2Speed = abs(motor2Speed);
motor4Speed = abs(motor4Speed);
} else if (rightY > 128) {
// S: motor2 rvs, motor4 rvs
motor2Speed = -abs(motor2Speed);
motor4Speed = -abs(motor4Speed);
} else {
// Stop motors if joystick is in the center
motor2Speed = 0;
motor4Speed = 0;
}
// Vertical drive control
if (ps2x.Button(PSB_L1)) {
// L1: motor5 fwd
motor5Speed = maxSpeed;
} else if (ps2x.Button(PSB_L2)) {
// L2: motor5 rvs
motor5Speed = -maxSpeed;
}
if (ps2x.Button(PSB_R1)) {
// R1: motor6 fwd
motor6Speed = maxSpeed;
} else if (ps2x.Button(PSB_R2)) {
// R2: motor6 rvs
motor6Speed = -maxSpeed;
}
// Send updated motor control commands to JHEMCU via SBUS
// Send SBUS commands for each channel
sendSBUSCommand(0, motor1Speed);
sendSBUSCommand(1, motor2Speed);
sendSBUSCommand(2, motor3Speed);
sendSBUSCommand(3, motor4Speed);
sendSBUSCommand(4, motor5Speed);
sendSBUSCommand(5, motor6Speed);
/*// Send PWM signals for motor control
sendPWM(0, abs(motor1Speed));
sendPWM(1, abs(motor2Speed));
sendPWM(2, abs(motor3Speed));
sendPWM(3, abs(motor4Speed));
sendPWM(4, abs(motor5Speed));
sendPWM(5, abs(motor6Speed));*/
// Print motor values for debugging
Serial.print("Motor 1: ");
Serial.print(motor1Speed);
Serial.print(" | Motor 2: ");
Serial.print(motor2Speed);
Serial.print(" | Motor 3: ");
Serial.println(motor3Speed);
Serial.print("Motor 4: ");
Serial.print(motor4Speed);
Serial.print(" | Motor 5: ");
Serial.print(motor5Speed);
Serial.print(" | Motor 6: ");
Serial.println(motor6Speed);
delay(20); //for stability
}
void sendSBUSCommand(int channel, int value) {
// SBUS protocol sends 16 channels of 11 bit data
static uint8_t sbusData[25] = {0x0F, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00};
// Calculate the byte index (each channel takes up 11 bits)
int byteIndex = 1 + (channel * 11) / 8;
int bitIndex = (channel * 11) % 8;
// Clear channel bits first (11 bits per channel)
for (int i = 0; i < 11; i++) {
sbusData[byteIndex + (bitIndex + i) / 8] &= ~(1 << ((bitIndex + i) % 8));
}
// Set bits according to value
for (int i = 0; i < 11; i++) {
if (value & (1 << i)) {
sbusData[byteIndex + (bitIndex + i) / 8] |= (1 << ((bitIndex + i) % 8));
}
}
// Send data
SBUS_SERIAL.write(sbusData, 25);
}
void processSBUSData() {
Serial.print("Available bytes: ");
Serial.println(SBUS_SERIAL.available());
uint8_t sbusData[25];
// Read SBUS data
const int minBytes = 10; // Set to an appropriate minimum value
if (SBUS_SERIAL.available() >= minBytes) {
// Continue with reading and parsing
//}
//if (SBUS_SERIAL.available() >= 25) {
SBUS_SERIAL.readBytes(sbusData, 25);
// Parse SBUS data and extract information
// Adjust these values based on your SBUS protocol
int channel1 = ((sbusData[2] | sbusData[3] << 8) & 0x07FF);
int channel2 = ((sbusData[4] | sbusData[5] << 8) & 0x07FF);
int channel3 = ((sbusData[6] | sbusData[7] << 8) & 0x07FF);
int channel4 = ((sbusData[8] | sbusData[9] << 8) & 0x07FF);
int channel5 = ((sbusData[10] | sbusData[11] << 8) & 0x07FF);
int channel6 = ((sbusData[12] | sbusData[13] << 8) & 0x07FF);
// ... repeat for other channels
// Print channel values for debugging
Serial.print("Channel 1: ");
Serial.println(channel1);
Serial.print("Channel 2: ");
Serial.println(channel2);
Serial.print("Channel 3: ");
Serial.println(channel3);
Serial.print("Channel 4: ");
Serial.println(channel4);
Serial.print("Channel 5: ");
Serial.println(channel5);
Serial.print("Channel 6: ");
Serial.println(channel6);
}
}