Hi,
I am using an arduino UNO, USB Shield board and a motor shield board to control a robot's forward and backwards movement as well as controlling 2 servo motors, through a PS4 DualShock controller. I currently have it working but I'm looking to make the controls easier to use. At the moment I am using the direction buttons to control forward and backwards movement and the square/circle/triangle/cross buttons to control the servos. However, I want to use the left joystick to control the forward/backward movement and the right joystick to control the servos, but I don't know how to do this. Below is my current code.
Thank you in advance.
#include <PS4BT.h>
#include <UsbCore.h>
#include <AFMotor.h>
#include <Servo.h>
#ifdef dobogusinclude
#include <spi4teensy3.h>
#include <SPI.h>
#endif
// create servo object to control a servo
Servo servo1; // 1 is for horizontal movement - pins closest to edge of board
int pos1 = 90;
Servo servo2; // 2 is for vertical movement - pins closest to motor output
int pos2 = 90;
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);
bool printAngle, printTouch;
uint8_t oldL2Value, oldR2Value;
// create motor shield object
AF_DCMotor motor1(1);
AF_DCMotor motor2(2);
//AF_DCMotor motor3(3);
AF_DCMotor motor4(4);
void setup() {
// initialize serial communication
Serial.begin(115200);
// turn on motor
motor1.setSpeed(100);motor2.setSpeed(100);motor4.setSpeed(100);//motor3.setSpeed(100);
motor1.run(RELEASE);motor2.run(RELEASE);motor4.run(RELEASE);//motor3.run(RELEASE);
servo1.attach(10);
servo2.attach(9);
#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() {
// read input from PS4 controller
Usb.Task();
uint8_t i;
if (PS4.connected()) {
if (PS4.getButtonClick(UP)) {
motor1.run(FORWARD);motor2.run(FORWARD);motor4.run(FORWARD);//motor4.run(FORWARD);
for (i=0; i<255; i++) {
motor1.setSpeed(i);motor2.setSpeed(i);motor4.setSpeed(i);//motor4.setSpeed(i);
delay(1);
}
} if (PS4.getButtonClick(RIGHT)) {
motor1.run(RELEASE);motor2.run(RELEASE);motor4.run(RELEASE);//motor4.run(RELEASE);
} if (PS4.getButtonClick(DOWN)) {
motor1.run(BACKWARD);motor2.run(BACKWARD);motor4.run(BACKWARD);//motor4.run(BACKWARD);
for (i=0; i<255; i++) {
motor1.setSpeed(i);motor2.setSpeed(i);motor4.setSpeed(i);//motor4.setSpeed(i);
delay(1);
}
} if (PS4.getButtonClick(LEFT)) {
Serial.print(F("\r\nLeft"));
PS4.setLed(Green);
}
if (PS4.getButtonPress(CIRCLE)) {
pos1 += 1;
if (pos1 > 180){
pos1 = 180;
}
servo1.write(pos1);
delay(1);
}
if (PS4.getButtonPress(SQUARE)) {
pos1 -= 1;
if (pos1 < 0){
pos1 = 0;
}
servo1.write(pos1);
delay(1);
}
if (PS4.getButtonPress(TRIANGLE)) {
pos2 = pos2 + 1;
if (pos2 > 180){
pos2 = 180;
}
servo2.write(pos2);
delay(1);
}
if (PS4.getButtonPress(CROSS)) {
pos2 -= 1;
if (pos2 < 0){
pos2 = 0;
}
servo2.write(pos2);
delay(1);
}
}
}