Hi there,
I am currently working on a robot arm project and I am trying to control it wirelessly with a PS3 controller. I am using a R4 Minima with a USB Host Shield and bluetooth dongle.
I managed to control the CAN Bus wirelessly, but the Servo library uses Pin 9 and 10 which the USB Host shield also uses.
I've tried other Servo libraries but to no avail. Any guidance is greatly appreciated. Essentially I need a setup that allows me to control a CAN Bus and 1 Servo with a wireless PS3 controller.
/*
Example sketch for the PS3 Bluetooth library - developed by Kristian Lauszus
For more information visit my blog: http://blog.tkjelectronics.dk/ or
send me an e-mail: kristianl@tkjelectronics.com
*/
#include <Arduino_CAN.h>
#include <PS3BT.h>
//#include <Servo.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>
//Joystick Variables
float X_raw = 0;
float Y_raw = 0;
float Z_raw = 0;
float A_raw = 0;
float B_raw = 0;
float C_raw = 0;
int X_pos;
int Y_pos;
int Z_pos;
int A_pos;
int B_pos;
int C_pos;
// Servo Variables
//Servo myservo; // create servo object to control a servo
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 class in two ways */
PS3BT PS3(&Btd); // This will just create the instance
//PS3BT PS3(&Btd, 0x00, 0x15, 0x83, 0x3D, 0x0A, 0x57); // This will also store the bluetooth address - this can be obtained from the dongle when running the sketch
bool printTemperature, printAngle;
void setup() {
Serial.begin(115200);
#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\nPS3 Bluetooth Library Started"));
while (!Serial) {}
if (!CAN.begin(CanBitRate::BR_500k)) {
Serial.println("CAN.begin(...) failed.");
for (;;) {}
}
//Gripper
//myservo.attach(2); // attaches the servo on pin 9 to the servo object
}
static uint32_t msg_cnt = 0;
void loop() {
//position("01","CCW",10,10,10);
Usb.Task();
if (PS3.PS3Connected || PS3.PS3NavigationConnected) {
if (PS3.getAnalogHat(LeftHatX) > 128 || PS3.getAnalogHat(LeftHatX) < 127 || PS3.getAnalogHat(LeftHatY) > 128 || PS3.getAnalogHat(LeftHatY) < 127 || PS3.getAnalogHat(RightHatX) > 128 || PS3.getAnalogHat(RightHatX) < 127 || PS3.getAnalogHat(RightHatY) > 128 || PS3.getAnalogHat(RightHatY) < 127) {
//Serial.print(F("\r\nLeftHatX: "));
//Serial.print(PS3.getAnalogHat(LeftHatX));
//Serial.print(F("\tLeftHatY: "));
//Serial.print(PS3.getAnalogHat(LeftHatY));
/*if (PS3.PS3Connected) { // The Navigation controller only have one joystick
Serial.print(F("\tRightHatX: "));
Serial.print(PS3.getAnalogHat(RightHatX));
Serial.print(F("\tRightHatY: "));
Serial.print(PS3.getAnalogHat(RightHatY));
}*/
//X Axis
X_raw = PS3.getAnalogHat(LeftHatX);
int X_max = 100;
if (X_raw > 128) {
X_pos = abs(int(((X_raw - 128) / 128) * X_max)) + 1;
} else {
X_pos = abs(int((1 - (X_raw / 128.0)) * X_max));
}
if (X_raw > 128) {
if (X_pos > 10 && X_pos <= X_max) {
speed("01", "CW", X_pos, 0);
Serial.print(F("\r\nX: +"));
Serial.print(X_pos);
} else {
speed("01", "CW", 0, 0);
Serial.print(F("\r\nX: 0"));
}
} else {
if (X_pos > 10 && X_pos <= X_max) {
speed("01", "CCW", X_pos, 0);
Serial.print(F("\r\nX: -"));
Serial.print(X_pos);
} else {
speed("01", "CCW", 0, 0);
Serial.print(F("\r\nX: 0"));
}
}
//Y Axis
Y_raw = PS3.getAnalogHat(LeftHatY);
int Y_max = 150;
if (Y_raw > 128) {
Y_pos = abs(int(((Y_raw - 128) / 128) * Y_max)) + 2;
} else {
Y_pos = abs(int((1 - (Y_raw / 128.0)) * Y_max));
}
if (Y_raw > 128) {
if (Y_pos > 10 && Y_pos <= Y_max) {
speed("02", "CCW", Y_pos, 0);
Serial.print(F("\tY: +"));
Serial.print(Y_pos);
} else {
speed("02", "CCW", 0, 0);
Serial.print(F("\tY: 0"));
}
} else {
if (Y_pos > 10 && Y_pos <= Y_max) {
speed("02", "CW", Y_pos, 0);
Serial.print(F("\tY: -"));
Serial.print(Y_pos);
} else {
speed("02", "CW", 0, 0);
Serial.print(F("\tY: 0"));
}
}
/*if (PS3.getButtonClick(L1)) {
//B Axis
B_raw = PS3.getAnalogHat(RightHatY);
int B_max = 200;
if (B_raw > 128) {
B_pos = abs(int(((B_raw - 128) / 128) * B_max));
//Serial.println(Z_pos);
} else {
B_pos = abs(int((1 - (B_raw / 128.0)) * B_max));
}
if (B_raw > 128) {
if (B_pos > 10 && B_pos <= B_max) {
speed("05", "CW", B_pos, 0);
} else {
speed("05", "CW", 0, 0);
}
} else {
if (B_pos > 10 && B_pos <= B_max) {
speed("05", "CCW", B_pos, 0);
} else {
speed("05", "CCW", 0, 0);
}
}
//C Axis
C_raw = PS3.getAnalogHat(RightHatX);
int C_max = 400;
if (C_raw > 128) {
C_pos = abs(int(((C_raw - 128) / 128) * C_max));
//Serial.println(A_pos);
} else {
C_pos = abs(int((1 - (C_raw / 128.0)) * C_max));
}
if (C_raw > 128) {
if (C_pos > 10 && C_pos <= C_max) {
speed("06", "CW", C_pos, 0);
} else {
speed("06", "CW", 0, 0);
}
} else {
if (C_pos > 10 && C_pos <= C_max) {
speed("06", "CCW", C_pos, 0);
} else {
speed("06", "CCW", 0, 0);
}
}
} else {*/
//Z Axis
Z_raw = PS3.getAnalogHat(RightHatY);
int Z_max = 200;
if (Z_raw > 128) {
Z_pos = abs(int(((Z_raw - 128) / 128) * Z_max)) + 2;
//Serial.println(Z_pos);
} else {
Z_pos = abs(int((1 - (Z_raw / 128.0)) * Z_max));
}
if (Z_raw > 128) {
if (Z_pos > 10 && Z_pos <= Z_max) {
speed("03", "CW", Z_pos, 0);
Serial.print(F("\tZ: +"));
Serial.print(Z_pos);
} else {
speed("03", "CW", 0, 0);
Serial.print(F("\tZ: 0"));
}
} else {
if (Z_pos > 10 && Z_pos <= Z_max) {
speed("03", "CCW", Z_pos, 0);
Serial.print(F("\tZ: -"));
Serial.print(Z_pos);
} else {
speed("03", "CCW", 0, 0);
Serial.print(F("\tZ: 0"));
}
}
//A Axis
A_raw = PS3.getAnalogHat(RightHatX);
int A_max = 400;
if (A_raw > 128) {
A_pos = abs(int(((A_raw - 128) / 128) * A_max)) + 4;
//Serial.println(A_pos);
} else {
A_pos = abs(int((1 - (A_raw / 128.0)) * A_max));
}
if (A_raw > 128) {
if (A_pos > 10 && A_pos <= A_max) {
speed("04", "CW", A_pos, 0);
Serial.print(F("\tA: +"));
Serial.print(A_pos);
} else {
speed("04", "CW", 0, 0);
Serial.print(F("\tA: 0"));
}
} else {
if (A_pos > 10 && A_pos <= A_max) {
speed("04", "CCW", A_pos, 0);
Serial.print(F("\tA: -"));
Serial.print(A_pos);
} else {
speed("04", "CCW", 0, 0);
Serial.print(F("\tA: 0"));
}
}
//}
} else {
speed("01", "CW", 0, 0);
speed("02", "CW", 0, 0);
speed("03", "CW", 0, 0);
speed("04", "CW", 0, 0);
speed("05", "CW", 0, 0);
speed("06", "CW", 0, 0);
}
// Analog button values can be read from almost all buttons
if (PS3.getAnalogButton(L2) || PS3.getAnalogButton(R2)) {
Serial.print(F("\r\nL2: "));
Serial.print(PS3.getAnalogButton(L2));
if (PS3.PS3Connected) {
Serial.print(F("\tR2: "));
Serial.print(PS3.getAnalogButton(R2));
}
}
if (PS3.getButtonClick(PS)) {
Serial.print(F("\r\nPS"));
PS3.disconnect();
} else {
if (PS3.getButtonClick(TRIANGLE)) {
Serial.print(F("\r\nTriangle"));
PS3.setRumbleOn(RumbleLow);
}
if (PS3.getButtonClick(CIRCLE)) {
Serial.print(F("\r\nCircle"));
//PS3.setRumbleOn(RumbleHigh);
//Flag Waving Sequence
speed("04", "CW", 200, 0);
delay(1000);
for (int x = 0; x < 3; x++) {
speed("01", "CW", 50, 0);
speed("04", "CCW", 200, 0);
delay(2000);
speed("01", "CCW", 50, 0);
speed("04", "CW", 200, 0);
delay(2000);
}
speed("01", "CCW", 0, 0);
speed("04", "CCW", 200, 0);
delay(1000);
speed("04", "CCW", 0, 0);
}
if (PS3.getButtonClick(CROSS))
Serial.print(F("\r\nCross"));
if (PS3.getButtonClick(SQUARE))
Serial.print(F("\r\nSquare"));
if (PS3.getButtonClick(UP)) {
Serial.print(F("\r\nUp"));
if (PS3.PS3Connected) {
PS3.setLedOff();
PS3.setLedOn(LED4);
}
}
if (PS3.getButtonClick(RIGHT)) {
Serial.print(F("\r\nRight"));
if (PS3.PS3Connected) {
PS3.setLedOff();
PS3.setLedOn(LED1);
}
}
if (PS3.getButtonClick(DOWN)) {
Serial.print(F("\r\nDown"));
if (PS3.PS3Connected) {
PS3.setLedOff();
PS3.setLedOn(LED2);
}
}
if (PS3.getButtonClick(LEFT)) {
Serial.print(F("\r\nLeft"));
if (PS3.PS3Connected) {
PS3.setLedOff();
PS3.setLedOn(LED3);
}
}
if (PS3.getButtonClick(L1))
Serial.print(F("\r\nL1"));
if (PS3.getButtonClick(L3))
Serial.print(F("\r\nL3"));
if (PS3.getButtonClick(R1))
Serial.print(F("\r\nR1"));
if (PS3.getButtonClick(R3))
Serial.print(F("\r\nR3"));
if (PS3.getButtonClick(SELECT)) {
Serial.print(F("\r\nSelect - "));
PS3.printStatusString();
}
if (PS3.getButtonClick(START)) {
Serial.print(F("\r\nStart"));
printAngle = !printAngle;
}
}
#if 0 // Set this to 1 in order to see the angle of the controller
if (printAngle) {
Serial.print(F("\r\nPitch: "));
Serial.print(PS3.getAngle(Pitch));
Serial.print(F("\tRoll: "));
Serial.print(PS3.getAngle(Roll));
}
#endif
}
#if 0 // Set this to 1 in order to enable support for the Playstation Move controller
else if (PS3.PS3MoveConnected) {
if (PS3.getAnalogButton(T)) {
Serial.print(F("\r\nT: "));
Serial.print(PS3.getAnalogButton(T));
}
if (PS3.getButtonClick(PS)) {
Serial.print(F("\r\nPS"));
PS3.disconnect();
}
else {
if (PS3.getButtonClick(SELECT)) {
Serial.print(F("\r\nSelect"));
printTemperature = !printTemperature;
}
if (PS3.getButtonClick(START)) {
Serial.print(F("\r\nStart"));
printAngle = !printAngle;
}
if (PS3.getButtonClick(TRIANGLE)) {
Serial.print(F("\r\nTriangle"));
PS3.moveSetBulb(Red);
}
if (PS3.getButtonClick(CIRCLE)) {
Serial.print(F("\r\nCircle"));
PS3.moveSetBulb(Green);
}
if (PS3.getButtonClick(SQUARE)) {
Serial.print(F("\r\nSquare"));
PS3.moveSetBulb(Blue);
}
if (PS3.getButtonClick(CROSS)) {
Serial.print(F("\r\nCross"));
PS3.moveSetBulb(Yellow);
}
if (PS3.getButtonClick(MOVE)) {
PS3.moveSetBulb(Off);
Serial.print(F("\r\nMove"));
Serial.print(F(" - "));
PS3.printStatusString();
}
}
if (printAngle) {
Serial.print(F("\r\nPitch: "));
Serial.print(PS3.getAngle(Pitch));
Serial.print(F("\tRoll: "));
Serial.print(PS3.getAngle(Roll));
}
else if (printTemperature) {
Serial.print(F("\r\nTemperature: "));
Serial.print(PS3.getTemperature());
}
}
#endif
}
void home(String CanID) {
msg_cnt = 0;
//CAN ID
String Byte0 = CanID;
String Byte0HexString = "0x" + Byte0;
uint32_t Byte0Hex = strtoul(Byte0HexString.c_str(), NULL, 0);
//Command Code
String Byte1 = "91";
String Byte1HexString = "0x" + Byte1;
uint32_t Byte1Hex = strtoul(Byte1HexString.c_str(), NULL, 0);
//CRC
String Byte2 = crc(Byte0 + Byte1);
String Byte2HexString = "0x" + Byte2;
uint32_t Byte2Hex = strtoul(Byte2HexString.c_str(), NULL, 0);
/* Assemble a CAN message with the format of
* 0xCA 0xFE 0x00 0x00 [4 byte message counter]
*/
uint8_t const msg_data[] = { Byte1Hex, Byte2Hex, 0, 0, 0, 0 };
memcpy((void *)(msg_data + 5), &msg_cnt, sizeof(msg_cnt));
CanMsg const msg(CanStandardId(Byte0Hex), 1, msg_data);
//04 FD 80 78 00 00 4E 20 67
/* Transmit the CAN message, capture and display an
* error core in case of failure.
*/
if (int const rc = CAN.write(msg); rc < 0) {
//Serial.print("CAN.write(...) failed with error code ");
//Serial.println(rc);
//for (;;) { }
}
/* Increase the message counter. */
msg_cnt++;
/* Only send one message per second. */
//delay(1000);
}
void position(String CanID, String dir, unsigned int speed, unsigned int acc, unsigned int pulses) {
//Serial.println("position1");
msg_cnt = 0;
//CAN ID
String Byte0 = CanID;
String Byte0HexString = "0x" + Byte0;
uint32_t Byte0Hex = strtoul(Byte0HexString.c_str(), NULL, 0);
//Command Code
String Byte1 = "FD";
String Byte1HexString = "0x" + Byte1;
uint32_t Byte1Hex = strtoul(Byte1HexString.c_str(), NULL, 0);
//Direction (Byte 2 b7) and Speed (Byte 2 b0 to b3 + byte 3)
String byteDir = binarydir(dir);
while (byteDir.length() < 8 - (binaryspeed(speed).length() - 8)) {
byteDir = byteDir + "0";
}
String Byte2 = binaryToHex(String(byteDir + binaryspeed(speed)).substring(0, 8));
String Byte2HexString = "0x" + Byte2;
uint32_t Byte2Hex = strtoul(Byte2HexString.c_str(), NULL, 0);
String Byte3 = binaryToHex(String(byteDir + binaryspeed(speed)).substring(8, 16));
String Byte3HexString = "0x" + Byte3;
uint32_t Byte3Hex = strtoul(Byte3HexString.c_str(), NULL, 0);
//Acceleration
String Byte4 = intToHex(acc);
String Byte4HexString = "0x" + Byte4;
uint32_t Byte4Hex = strtoul(Byte4HexString.c_str(), NULL, 0);
//Pulses
// 1 fullstep is 1.8 degrees, assuming the configured mStep = 16 (default), 1 mStep is 1.8/16 = 0.1125degrees
//Therefore 1 rotation is 360/0.1125 = 3200pulses
String bytePulses = intToHex(pulses);
while (bytePulses.length() < 6) {
bytePulses = "0" + bytePulses;
}
String Byte5 = bytePulses.substring(0, 2);
String Byte5HexString = "0x" + Byte5;
uint32_t Byte5Hex = strtoul(Byte5HexString.c_str(), NULL, 0);
String Byte6 = bytePulses.substring(2, 4);
String Byte6HexString = "0x" + Byte6;
uint32_t Byte6Hex = strtoul(Byte6HexString.c_str(), NULL, 0);
String Byte7 = bytePulses.substring(4, 6);
String Byte7HexString = "0x" + Byte7;
uint32_t Byte7Hex = strtoul(Byte7HexString.c_str(), NULL, 0);
//CRC
String Byte8 = crc(Byte0 + Byte1 + Byte2 + Byte3 + Byte4 + Byte5 + Byte6 + Byte7);
String Byte8HexString = "0x" + Byte8;
uint32_t Byte8Hex = strtoul(Byte8HexString.c_str(), NULL, 0);
/* Assemble a CAN message with the format of
* 0xCA 0xFE 0x00 0x00 [4 byte message counter]
*/
uint8_t const msg_data[] = { Byte1Hex, Byte2Hex, Byte3Hex, Byte4Hex, Byte5Hex, Byte6Hex, Byte7Hex, Byte8Hex };
memcpy((void *)(msg_data + 8), &msg_cnt, sizeof(msg_cnt));
CanMsg const msg(CanStandardId(Byte0Hex), 8, msg_data);
//04 FD 80 78 00 00 4E 20 67
/* Transmit the CAN message, capture and display an
* error core in case of failure.
*/
if (int const rc = CAN.write(msg); rc < 0) {
//Serial.print("CAN.write(...) failed with error code ");
//Serial.println(rc);
//for (;;) { }
}
/* Increase the message counter. */
msg_cnt++;
/* Only send one message per second. */
//delay(1000);
}
void speed(String CanID, String dir, unsigned int speed, unsigned int acc) {
msg_cnt = 0;
//CAN ID
String Byte0 = CanID;
String Byte0HexString = "0x" + Byte0;
uint32_t Byte0Hex = strtoul(Byte0HexString.c_str(), NULL, 0);
//Command Code
String Byte1 = "F6";
String Byte1HexString = "0x" + Byte1;
uint32_t Byte1Hex = strtoul(Byte1HexString.c_str(), NULL, 0);
//Direction (Byte 2 b7) and Speed (Byte 2 b0 to b3 + byte 3)
String byteDir = binarydir(dir);
while (byteDir.length() < 8 - (binaryspeed(speed).length() - 8)) {
byteDir = byteDir + "0";
}
String Byte2 = binaryToHex(String(byteDir + binaryspeed(speed)).substring(0, 8));
String Byte2HexString = "0x" + Byte2;
uint32_t Byte2Hex = strtoul(Byte2HexString.c_str(), NULL, 0);
String Byte3 = binaryToHex(String(byteDir + binaryspeed(speed)).substring(8, 16));
String Byte3HexString = "0x" + Byte3;
uint32_t Byte3Hex = strtoul(Byte3HexString.c_str(), NULL, 0);
//Acceleration
String Byte4 = intToHex(acc);
String Byte4HexString = "0x" + Byte4;
uint32_t Byte4Hex = strtoul(Byte4HexString.c_str(), NULL, 0);
//CRC
String Byte5 = crc(Byte0 + Byte1 + Byte2 + Byte3 + Byte4);
String Byte5HexString = "0x" + Byte5;
uint32_t Byte5Hex = strtoul(Byte5HexString.c_str(), NULL, 0);
/* Assemble a CAN message with the format of
* 0xCA 0xFE 0x00 0x00 [4 byte message counter]
*/
uint8_t const msg_data[] = { Byte1Hex, Byte2Hex, Byte3Hex, Byte4Hex, Byte5Hex };
memcpy((void *)(msg_data + 5), &msg_cnt, sizeof(msg_cnt));
CanMsg const msg(CanStandardId(Byte0Hex), sizeof(msg_data), msg_data);
//04 FD 80 78 00 00 4E 20 67
/* Transmit the CAN message, capture and display an
* error core in case of failure.
*/
//CAN.write(msg);
if (int const rc = CAN.write(msg); rc < 0) {
//Serial.print("CAN.write(...) failed with error code ");
//Serial.println(rc);
//for (;;) { }
}
/* Increase the message counter. */
msg_cnt++;
//msg_cnt = 0;
/* Only send one message per second. */
//delay(1000);
}
//Function to convert speed to binary
String binaryspeed(unsigned int speed) {
String binaryString = String(speed, BIN);
// Add leading zeros if necessary
while (binaryString.length() < 8) {
binaryString = "0" + binaryString;
}
return (binaryString);
}
//Function to convert "CCW" and "CW" to 0 and 1
String binarydir(String dir) {
if (dir == "CW") {
return "0";
}
if (dir == "CCW") {
return "1";
}
}
//Function to calculate CRC using checksum8
String crc(const String &hexString) {
unsigned char checksum = checksum8(hexString);
String CRC = String(checksum, HEX);
CRC.toUpperCase();
String Command = (hexString + CRC);
//return Command;
return CRC;
}
//checksum8 Function for CRC use
unsigned char checksum8(const String &hexString) {
String hexStringNoSpaces = hexString;
hexStringNoSpaces.replace(" ", ""); // remove spaces
unsigned char sum = 0;
for (int i = 0; i < hexStringNoSpaces.length(); i += 2) {
unsigned char high = hexToInt(hexStringNoSpaces[i]);
unsigned char low = hexToInt(hexStringNoSpaces[i + 1]);
unsigned char byte = (high << 4) | low;
sum += byte;
}
return sum;
}
//Fucntion to convert HEX to Integer
int hexToInt(char c) {
if (c >= '0' && c <= '9') return c - '0';
if (c >= 'A' && c <= 'F') return c - 'A' + 10;
if (c >= 'a' && c <= 'f') return c - 'a' + 10;
return 0;
}
//Function to convert Binary to HEX
String binaryToHex(String binaryString) {
int decimalValue = 0;
for (int i = 0; i < 8; i++) {
if (binaryString[i] == '1') {
decimalValue += (1 << (7 - i));
}
}
char hexValue[3];
sprintf(hexValue, "%02X", decimalValue);
return String(hexValue);
}
//Function to convert Integer to HEX
String intToHex(int value) {
char hexString[3];
sprintf(hexString, "%02X", value);
return String(hexString);
}