To anyone else who stumbles upon this post, switching to an R4 Minima did in fact work for me due to the pins no longer conflicting. Here's the code (just combined the examples from Odrive and USB Host libraries):
#include <Arduino.h>
#include "ODriveCAN.h"
#include <PS5BT.h>
#include <usbhub.h>
// Documentation for this example can be found here:
// https://docs.odriverobotics.com/v/latest/guides/arduino-can-guide.html
/* Configuration of example sketch -------------------------------------------*/
// CAN bus baudrate. Make sure this matches for every device on the bus
#define CAN_BAUDRATE 1000000
// ODrive node_id for odrv0
#define ODRV0_NODE_ID 0
#define ODRV1_NODE_ID 1
#define ODRV2_NODE_ID 2
#define ODRV3_NODE_ID 3
// Uncomment below the line that corresponds to your hardware.
// See also "Board-specific settings" to adapt the details for your hardware setup.
// #define IS_TEENSY_BUILTIN // Teensy boards with built-in CAN interface (e.g. Teensy 4.1). See below to select which interface to use.
#define IS_ARDUINO_BUILTIN // Arduino boards with built-in CAN interface (e.g. Arduino Uno R4 Minima)
// #define IS_MCP2515 // Any board with external MCP2515 based extension module. See below to configure the module.
/* Board-specific includes ---------------------------------------------------*/
#if defined(IS_TEENSY_BUILTIN) + defined(IS_ARDUINO_BUILTIN) + defined(IS_MCP2515) != 1
#warning "Select exactly one hardware option at the top of this file."
#if CAN_HOWMANY > 0 || CANFD_HOWMANY > 0
#define IS_ARDUINO_BUILTIN
#warning "guessing that this uses HardwareCAN"
#else
#error "cannot guess hardware version"
#endif
#endif
#ifdef IS_ARDUINO_BUILTIN
// See https://github.com/arduino/ArduinoCore-API/blob/master/api/HardwareCAN.h
// and https://github.com/arduino/ArduinoCore-renesas/tree/main/libraries/Arduino_CAN
#include <Arduino_CAN.h>
#include <ODriveHardwareCAN.hpp>
#endif // IS_ARDUINO_BUILTIN
#ifdef IS_MCP2515
// See https://github.com/sandeepmistry/arduino-CAN/
#include "MCP2515.h"
#include "ODriveMCPCAN.hpp"
#endif // IS_MCP2515
#ifdef IS_TEENSY_BUILTIN
// See https://github.com/tonton81/FlexCAN_T4
// clone https://github.com/tonton81/FlexCAN_T4.git into /src
#include <FlexCAN_T4.h>
#include "ODriveFlexCAN.hpp"
struct ODriveStatus; // hack to prevent teensy compile error
#endif // IS_TEENSY_BUILTIN
// Satisfy the IDE, which needs to see the include statment in the ino too.
#ifdef dobogusinclude
#include <spi4teensy3.h>
#endif
#include <SPI.h>
/* Board-specific settings ---------------------------------------------------*/
/* Teensy */
#ifdef IS_TEENSY_BUILTIN
FlexCAN_T4<CAN1, RX_SIZE_256, TX_SIZE_16> can_intf;
bool setupCan() {
can_intf.begin();
can_intf.setBaudRate(CAN_BAUDRATE);
can_intf.setMaxMB(16);
can_intf.enableFIFO();
can_intf.enableFIFOInterrupt();
can_intf.onReceive(onCanMessage);
return true;
}
#endif // IS_TEENSY_BUILTIN
/* MCP2515-based extension modules -*/
#ifdef IS_MCP2515
MCP2515Class& can_intf = CAN;
// chip select pin used for the MCP2515
#define MCP2515_CS 10
// interrupt pin used for the MCP2515
// NOTE: not all Arduino pins are interruptable, check the documentation for your board!
#define MCP2515_INT 2
// freqeuncy of the crystal oscillator on the MCP2515 breakout board.
// common values are: 16 MHz, 12 MHz, 8 MHz
#define MCP2515_CLK_HZ 8000000
static inline void receiveCallback(int packet_size) {
if (packet_size > 8) {
return; // not supported
}
CanMsg msg = {.id = (unsigned int)CAN.packetId(), .len = (uint8_t)packet_size};
CAN.readBytes(msg.buffer, packet_size);
onCanMessage(msg);
}
bool setupCan() {
// configure and initialize the CAN bus interface
CAN.setPins(MCP2515_CS, MCP2515_INT);
CAN.setClockFrequency(MCP2515_CLK_HZ);
if (!CAN.begin(CAN_BAUDRATE)) {
return false;
}
CAN.onReceive(receiveCallback);
return true;
}
#endif // IS_MCP2515
/* Arduinos with built-in CAN */
#ifdef IS_ARDUINO_BUILTIN
HardwareCAN& can_intf = CAN;
bool setupCan() {
return can_intf.begin((CanBitRate)CAN_BAUDRATE);
}
#endif
/* PS5 Setup */
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 PS5BT class in two ways */
// This will start an inquiry and then pair with the PS5 controller - you only have to do this once
// You will need to hold down the PS and Share button at the same time, the PS5 controller will then start to blink rapidly indicating that it is in pairing mode
//PS5BT PS5(&Btd, PAIR);
// After that you can simply create the instance like so and then press the PS button on the device
PS5BT PS5(&Btd);
bool printAngle = false, printTouch = false;
uint16_t lastMessageCounter = -1;
uint8_t player_led_mask = 0;
bool microphone_led = false;
uint32_t ps_timer;
/* Example sketch ------------------------------------------------------------*/
// Instantiate ODrive objects
ODriveCAN odrv0(wrap_can_intf(can_intf), ODRV0_NODE_ID);
ODriveCAN odrv1(wrap_can_intf(can_intf), ODRV1_NODE_ID);
ODriveCAN odrv2(wrap_can_intf(can_intf), ODRV2_NODE_ID);
ODriveCAN odrv3(wrap_can_intf(can_intf), ODRV3_NODE_ID); // Standard CAN message ID
ODriveCAN* odrives[] = {&odrv0, &odrv1, &odrv2, &odrv3}; // Make sure all ODriveCAN instances are accounted for here
struct ODriveUserData {
Heartbeat_msg_t last_heartbeat;
bool received_heartbeat = false;
Get_Encoder_Estimates_msg_t last_feedback;
bool received_feedback = false;
};
// Keep some application-specific user data for every ODrive.
ODriveUserData odrv0_user_data;
ODriveUserData odrv1_user_data;
ODriveUserData odrv2_user_data;
ODriveUserData odrv3_user_data;
// Called every time a Heartbeat message arrives from the ODrive
void onHeartbeat(Heartbeat_msg_t& msg, void* user_data) {
ODriveUserData* odrv_user_data = static_cast<ODriveUserData*>(user_data);
odrv_user_data->last_heartbeat = msg;
odrv_user_data->received_heartbeat = true;
}
// Called every time a feedback message arrives from the ODrive
void onFeedback(Get_Encoder_Estimates_msg_t& msg, void* user_data) {
ODriveUserData* odrv_user_data = static_cast<ODriveUserData*>(user_data);
odrv_user_data->last_feedback = msg;
odrv_user_data->received_feedback = true;
}
// Called for every message that arrives on the CAN bus
void onCanMessage(const CanMsg& msg) {
for (auto odrive: odrives) {
onReceive(msg, *odrive);
}
}
void setup() {
Serial.begin(115200);
// Wait for up to 3 seconds for the serial port to be opened on the PC side.
// If no PC connects, continue anyway.
for (int i = 0; i < 30 && !Serial; ++i) {
delay(100);
}
/* PS5 Code ------*/
#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.println(F("\r\nPS5 Bluetooth Library Started"));
/*---------*/
delay(200);
Serial.println("Starting ODriveCAN demo");
// Register callbacks for the heartbeat and encoder feedback messages
odrv0.onFeedback(onFeedback, &odrv0_user_data);
odrv0.onStatus(onHeartbeat, &odrv0_user_data);
odrv1.onFeedback(onFeedback, &odrv1_user_data);
odrv1.onStatus(onHeartbeat, &odrv1_user_data);
odrv2.onFeedback(onFeedback, &odrv2_user_data);
odrv2.onStatus(onHeartbeat, &odrv2_user_data);
odrv3.onFeedback(onFeedback, &odrv3_user_data);
odrv3.onStatus(onHeartbeat, &odrv3_user_data);
// Configure and initialize the CAN bus interface. This function depends on
// your hardware and the CAN stack that you're using.
if (!setupCan()) {
Serial.println("CAN failed to initialize: reset required");
while (true); // spin indefinitely
}
Serial.println("Waiting for ODrive0...");
while (!odrv0_user_data.received_heartbeat) {
pumpEvents(can_intf);
delay(100);
}
Serial.println("Waiting for ODrive1...");
while (!odrv1_user_data.received_heartbeat) {
pumpEvents(can_intf);
delay(100);
}
Serial.println("Waiting for ODrive2...");
while (!odrv2_user_data.received_heartbeat) {
pumpEvents(can_intf);
delay(100);
}
Serial.println("Waiting for ODrive3...");
while (!odrv3_user_data.received_heartbeat) {
pumpEvents(can_intf);
delay(100);
}
Serial.println("found ODrive");
// request bus voltage and current (1sec timeout)
Serial.println("attempting to read bus voltage and current");
Get_Bus_Voltage_Current_msg_t vbus;
if (!odrv1.request(vbus, 1)) {
Serial.println("vbus request failed!");
while (true); // spin indefinitely
}
Serial.print("DC voltage [V]: ");
Serial.println(vbus.Bus_Voltage);
Serial.print("DC current [A]: ");
Serial.println(vbus.Bus_Current);
Serial.println("Enabling closed loop control...");
while (odrv1_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) {
odrv1.clearErrors();
delay(1);
odrv1.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
// Pump events for 150ms. This delay is needed for two reasons;
// 1. If there is an error condition, such as missing DC power, the ODrive might
// briefly attempt to enter CLOSED_LOOP_CONTROL state, so we can't rely
// on the first heartbeat response, so we want to receive at least two
// heartbeats (100ms default interval).
// 2. If the bus is congested, the setState command won't get through
// immediately but can be delayed.
//Serial.println(odrv1_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
for (int i = 0; i < 25; ++i) {
delay(10);
pumpEvents(can_intf);
}
Serial.println("Loop1");
}
while (odrv2_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) {
odrv2.clearErrors();
delay(1);
odrv2.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
// Pump events for 150ms. This delay is needed for two reasons;
// 1. If there is an error condition, such as missing DC power, the ODrive might
// briefly attempt to enter CLOSED_LOOP_CONTROL state, so we can't rely
// on the first heartbeat response, so we want to receive at least two
// heartbeats (100ms default interval).
// 2. If the bus is congested, the setState command won't get through
// immediately but can be delayed.
for (int i = 0; i < 25; ++i) {
delay(10);
pumpEvents(can_intf);
}
Serial.println("Loop2");
}
while (odrv3_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) {
odrv3.clearErrors();
delay(1);
odrv3.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
//Serial.println(odrv3_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
// Pump events for 150ms. This delay is needed for two reasons;
// 1. If there is an error condition, such as missing DC power, the ODrive might
// briefly attempt to enter CLOSED_LOOP_CONTROL state, so we can't rely
// on the first heartbeat response, so we want to receive at least two
// heartbeats (100ms default interval).
// 2. If the bus is congested, the setState command won't get through
// immediately but can be delayed.
for (int i = 0; i < 25; ++i) {
delay(10);
pumpEvents(can_intf);
}
Serial.println("Loop3");
}
while (odrv0_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) {
odrv0.clearErrors();
delay(1);
odrv0.setState(ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL);
// Pump events for 150ms. This delay is needed for two reasons;
// 1. If there is an error condition, such as missing DC power, the ODrive might
// briefly attempt to enter CLOSED_LOOP_CONTROL state, so we can't rely
// on the first heartbeat response, so we want to receive at least two
// heartbeats (100ms default interval).
// 2. If the bus is congested, the setState command won't get through
// immediately but can be delayed.
for (int i = 0; i < 25; ++i) {
delay(10);
pumpEvents(can_intf);
}
Serial.println("Loop0");
}
Serial.println("ODrive running!");
}
void stopODrives(float current_position) {
odrv1.setPosition(current_position);
delay(10);
odrv0.setPosition(current_position);
delay(10);
odrv2.setPosition(current_position);
delay(10);
odrv3.setPosition(current_position);
delay(10);
}
void loop() {
pumpEvents(can_intf); // This is required on some platforms to handle incoming feedback CAN messages
static float current_position = 0.0f;
const float POSITION_INCREMENT = 1.0f; // Define how much the position changes with each key press
Usb.Task();
if (PS5.connected() && lastMessageCounter != PS5.getMessageCounter()) {
lastMessageCounter = PS5.getMessageCounter();
if (PS5.getAnalogHat(LeftHatX) > 137 || PS5.getAnalogHat(LeftHatX) < 117 || PS5.getAnalogHat(LeftHatY) > 137 || PS5.getAnalogHat(LeftHatY) < 117 || PS5.getAnalogHat(RightHatX) > 137 || PS5.getAnalogHat(RightHatX) < 117 || PS5.getAnalogHat(RightHatY) > 137 || PS5.getAnalogHat(RightHatY) < 117) {
Serial.print(F("\r\nLeftHatX: "));
Serial.print(PS5.getAnalogHat(LeftHatX));
Serial.print(F("\tLeftHatY: "));
Serial.print(PS5.getAnalogHat(LeftHatY));
Serial.print(F("\tRightHatX: "));
Serial.print(PS5.getAnalogHat(RightHatX));
Serial.print(F("\tRightHatY: "));
Serial.print(PS5.getAnalogHat(RightHatY));
}
if (PS5.getAnalogButton(L2) || PS5.getAnalogButton(R2)) { // These are the only analog buttons on the PS5 controller
Serial.print(F("\r\nL2: "));
Serial.print(PS5.getAnalogButton(L2));
Serial.print(F("\tR2: "));
Serial.print(PS5.getAnalogButton(R2));
}
// Set the left trigger to resist at the right trigger's level
static uint8_t oldR2Value = 0xFF;
if (PS5.getAnalogButton(R2) != oldR2Value) {
oldR2Value = PS5.getAnalogButton(R2);
PS5.leftTrigger.setTriggerForce(oldR2Value, 255);
}
// Hold the PS button for 1 second to disconnect the controller
// This prevents the controller from disconnecting when it is reconnected,
// as the PS button is sent when it reconnects
if (PS5.getButtonPress(PS)) {
if (millis() - ps_timer > 1000)
PS5.disconnect();
} else
ps_timer = millis();
if (PS5.getButtonClick(PS))
Serial.print(F("\r\nPS"));
if (PS5.getButtonClick(TRIANGLE)) {
Serial.print(F("\r\nTriangle"));
PS5.setRumbleOn(RumbleLow);
}
if (PS5.getButtonClick(CIRCLE)) {
Serial.print(F("\r\nCircle"));
PS5.setRumbleOn(RumbleHigh);
}
if (PS5.getButtonClick(CROSS)) {
Serial.print(F("\r\nCross"));
// Set the player LEDs
player_led_mask = (player_led_mask << 1) | 1;
if (player_led_mask > 0x1F)
player_led_mask = 0;
PS5.setPlayerLed(player_led_mask); // The bottom 5 bits set player LEDs
}
if (PS5.getButtonClick(SQUARE)) {
Serial.print(F("\r\nSquare"));
PS5.setRumbleOff();
}
if (PS5.getButtonClick(UP)) {
Serial.print(F("\r\nUp"));
PS5.setLed(Red);
current_position += POSITION_INCREMENT;
Serial.println("Forward");
odrv1.setPosition(current_position, -3);
delay(10);
odrv0.setPosition(current_position, 3);
delay(10);
odrv2.setPosition(current_position, -3);
delay(10);
odrv3.setPosition(current_position, 3);
delay(500);
stopODrives(current_position);
} if (PS5.getButtonClick(RIGHT)) {
Serial.print(F("\r\nRight"));
PS5.setLed(Blue);
} if (PS5.getButtonClick(DOWN)) {
Serial.print(F("\r\nDown"));
PS5.setLed(Yellow);
Serial.println("Backward");
current_position -= POSITION_INCREMENT;
odrv1.setPosition(current_position, 3);
delay(10);
odrv0.setPosition(current_position, -3);
delay(10);
odrv2.setPosition(current_position, 3);
delay(10);
odrv3.setPosition(current_position, -3);
delay(500);
stopODrives(current_position);
} if (PS5.getButtonClick(LEFT)) {
Serial.print(F("\r\nLeft"));
PS5.setLed(Green);
}
if (PS5.getButtonClick(L1))
Serial.print(F("\r\nL1"));
if (PS5.getButtonClick(L3))
Serial.print(F("\r\nL3"));
if (PS5.getButtonClick(R1))
Serial.print(F("\r\nR1"));
if (PS5.getButtonClick(R3))
Serial.print(F("\r\nR3"));
if (PS5.getButtonClick(CREATE))
Serial.print(F("\r\nCreate"));
if (PS5.getButtonClick(OPTIONS)) {
Serial.print(F("\r\nOptions"));
printAngle = !printAngle;
}
if (PS5.getButtonClick(TOUCHPAD)) {
Serial.print(F("\r\nTouchpad"));
printTouch = !printTouch;
}
if (PS5.getButtonClick(MICROPHONE)) {
Serial.print(F("\r\nMicrophone"));
microphone_led = !microphone_led;
PS5.setMicLed(microphone_led);
}
if (printAngle) { // Print angle calculated using the accelerometer only
Serial.print(F("\r\nPitch: "));
Serial.print(PS5.getAngle(Pitch));
Serial.print(F("\tRoll: "));
Serial.print(PS5.getAngle(Roll));
}
if (printTouch) { // Print the x, y coordinates of the touchpad
if (PS5.isTouching(0) || PS5.isTouching(1)) // Print newline and carriage return if any of the fingers are touching the touchpad
Serial.print(F("\r\n"));
for (uint8_t i = 0; i < 2; i++) { // The touchpad track two fingers
if (PS5.isTouching(i)) { // Print the position of the finger if it is touching the touchpad
Serial.print(F("X")); Serial.print(i + 1); Serial.print(F(": "));
Serial.print(PS5.getX(i));
Serial.print(F("\tY")); Serial.print(i + 1); Serial.print(F(": "));
Serial.print(PS5.getY(i));
Serial.print(F("\t"));
}
}
}
}
}