I am hoping someone can help me fix a lagging issue between a wireless PS4 controller and the MD25 motor controller. The motors currently move, but movement is a second or so behind button input.
Hardware:
- legit Sony PS4 controller.
- Arduino Uno
- USB host shield
- Bluetooth Dongle
- MD25 motor controller with the EMG30 motors (Devantech RD02 - 12 Volt Robot Drive System - RobotShop).
Thanks in advance for looking into this.
The following code contains the joysticks but it only uses L2 and R2 analog buttons for movement.
/*
PS4 and MD25 - Phil Eriksson
This is a modified version of the PS4 Bluetooth library - developed by Kristian Lauszus
For more information visit: http://blog.tkjelectronics.dk/
*/
#include <PS4BT.h>
#include <usbhub.h>
// Satisfy the IDE, which needs to see the include statment in the ino too.
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif
#include <SPI.h>
USB Usb;
//USBHub Hub1(&Usb); // Some dongles have a hub inside
BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
/* You can create the instance of the PS4BT class in two ways */
// This will start an inquiry and then pair with the PS4 controller - you only have to do this once
// You will need to hold down the PS and Share button at the same time, the PS4 controller will then start to blink rapidly indicating that it is in pairing mode
PS4BT PS4(&Btd, PAIR);
// After that you can simply create the instance like so and then press the PS button on the device
//PS4BT PS4(&Btd);
bool printAngle, printTouch;
uint8_t oldL2Value, oldR2Value;
int incomingByte;
void setup() {
Serial.begin(9600); //This needed to be changed from 112500 in the orignal sketch to match the MD25 requirements
#if !defined(MIPSEL)
while (!Serial); // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
#endif
if (Usb.Init() == -1) {
Serial.print(F("\r\nOSC did not start"));
while (1); // Halt
}
Serial.print(F("\r\nPS4 Bluetooth Library Started"));
}
void loop() {
Usb.Task();
Serial.write(0x00);
Serial.write(0x34);
Serial.write(1);
if (PS4.connected()) {
if (PS4.getAnalogHat(LeftHatX) > 137 || PS4.getAnalogHat(LeftHatX) < 117 || PS4.getAnalogHat(LeftHatY) > 137 || PS4.getAnalogHat(LeftHatY) < 117 || PS4.getAnalogHat(RightHatX) > 137 || PS4.getAnalogHat(RightHatX) < 117 || PS4.getAnalogHat(RightHatY) > 137 || PS4.getAnalogHat(RightHatY) < 117) {
Serial.print(F("\r\nLeftHatX: “));
Serial.print(PS4.getAnalogHat(LeftHatX));
Serial.print(F(”\tLeftHatY: “));
Serial.print(PS4.getAnalogHat(LeftHatY));
Serial.print(F(”\tRightHatX: “));
Serial.print(PS4.getAnalogHat(RightHatX));
Serial.print(F(”\tRightHatY: “));
Serial.print(PS4.getAnalogHat(RightHatY));
}
if (PS4.getAnalogButton(L2)>10) { // Forward
Serial.print(F(”\r\nL2: "));
Serial.print(PS4.getAnalogButton(L2));
//Set Speed motor 1
Serial.write(0x00);
Serial.write(0x31);
Serial.write(PS4.getAnalogButton(L2)/2);
//Set Speed motor 2
Serial.write(0x00);
Serial.write(0x32);
Serial.write(PS4.getAnalogButton(L2)/2);
}
if (PS4.getAnalogButton(R2)>10) { // Reverse
Serial.print(F("\tR2: "));
Serial.print(PS4.getAnalogButton(R2));
//Set Speed motor 1
Serial.write(0x00);
Serial.write(0x31);
Serial.write(PS4.getAnalogButton(R2)*-1/2);
//Set Speed motor 2
Serial.write(0x00);
Serial.write(0x32);
Serial.write(PS4.getAnalogButton(R2)*-1/2);
}
if (PS4.getAnalogButton(L2) != oldL2Value || PS4.getAnalogButton(R2) != oldR2Value) // Only write value if it's different
PS4.setRumbleOn(PS4.getAnalogButton(L2), PS4.getAnalogButton(R2));
oldL2Value = PS4.getAnalogButton(L2);
oldR2Value = PS4.getAnalogButton(R2);
if (PS4.getButtonClick(PS)) {
Serial.print(F("\r\nPS"));
PS4.disconnect();
}
incomingByte = Serial.read();
Serial.println(incomingByte);
}
}
