Hi, I am trying to write a C++ motor driver to control motor 1 and motor 2 on v4 robot host. I can successfully write and then read the battery voltage at A4 at register 0X130000 (ADC assigned to the battery level), and having some success to control one motor. Has anyone tried something similar or would want to collaborate? I am a cpp amateur.
Rationale
I am hoping to write C++ motor driver for Robot-HAT v4 attached to Raspberry pi 5 running Ubuntu 24.04 and ROS2 natively. Most things in ROS2 can be done with python, however ROS2 control requires C++ drivers etc hence my aim of replicating motor control in C++
Project aims
-
GPIO control via
libgpiodwhich is the recommended C/C++ library for GPIO control in Linux and raspberry pi 5 going forwards. GPIO pins required for control of motor direction -
PMW motor speed control via Linux kernel i2c device driver.
Issues so far:
I partly seem to have GPIO control using libgpiod library but having issues. Some people have reported issues with raspberry 5 and older versions of this library. The apt repositories installs version 1.6.x on Ubuntu but the library is up to version 2.x and apparently this works with the hardware changes between pi4 and pi5. I will install and build the v2 library and post back if I have success with this
As for PMW control, again I have this partially working and will post some code I’m using to test i2c control using C++ on my pi5
Some background information on GPIO control on pi 5:
So I gave up in libgpio v3 as it was too complicated for what I was trying to do. I wanted to create a test file in C++ to set GPIO pin to HIGH / LOW for motor-1 on the robot HAT v4 on a Raspberry Pi 5 running Ubuntu 24.04 LTS and ROS2. Instead I used wiringPi library that has been updated to work on the Raspberry Pi 5 (previously broken between pi 4 and pi 5 due to the new RP1 I/O controller)
Installing and WiringPi library onto Pi5:
Instructions: WiringPi/documentation/english/functions.md at master · WiringPi/WiringPi · GitHub
- Create Debian package:
sudo apt install git
git clone https://github.com/WiringPi/WiringPi.git
cd WiringPi
./build debian
mv debian-template/wiringpi_3.16_arm64.deb .
- Install package with apt
sudo apt install ./wiringpi_3.16_arm64.deb
- Uninstall if needed:
sudo apt purge wiringpi
Compiling C++ code with WiringPi library
- must add the
-lwiringPiflag at the end to include WiringPi library in the compiled code
g++ <yourProgram.cpp> -o <executableName> -lwiringPi
To run compiled executable file
- Terminal and from the directory you compiled your file:
sudo ./<executableFile>
- You will need to use sudo to give WringPi access to the GPIO
Simple C code to test WiringPi with user button and user LED
- Simple C code (copy and paste into IDE)
- Robot-HAT v4 (NOT v5) test user button (GPIO 25) to toggle user LED (GPIO 26)
#include <wiringPi.h>
#include <stdio.h> // for printf (analogous to std::cout in C++ using iostream)
#define LED_PIN 26 // wiringPi pin number for GPIO26
#define BUTTON_PIN 25 // wiringPi pin number for GPIO25
int main(void) {
// Initialize WiringPi using BCM (GPIO) numbering
if (wiringPiSetupGpio() == -1) {
printf("Error setting up wiringPi\n");
return 1;
}
printf("Press button to toggle LED, or ctrl+C to exit.\n");
printf("LED_PIN: %d, BUTTON_PIN: %d\n", LED_PIN, BUTTON_PIN);
//printf("LED_PIN: ", LED_PIN, ", BUTTON_PIN: " ,BUTTON_PIN );
pinMode(LED_PIN, OUTPUT); // Set LED pin as OUTPUT
pinMode(BUTTON_PIN, INPUT); // Set BUTTON pin as INPUT
pullUpDnControl(BUTTON_PIN, PUD_UP); // Enable pull-up resistor
int ledState = LOW, lastButtonState = HIGH;
printf("LED state: %d\n", ledState);
while (1) {
int buttonState = digitalRead(BUTTON_PIN);
if (buttonState == LOW && lastButtonState == HIGH) {
// Debounce delay
delay(50);
if (digitalRead(BUTTON_PIN) == LOW) { // confirm still pressed
ledState = !ledState;
digitalWrite(LED_PIN, ledState);
// Wait for button release
while (digitalRead(BUTTON_PIN) == LOW) { delay(10); }
}
}
lastButtonState = buttonState;
delay(10); // Small delay to ease CPU usage
//printf("Button pressed. LED state now: %d\n", ledState);
}
return 0;
}
- Save file with
.cextension and compile file withgccin terminal:
gcc <fileName.c> -o <executableName> -lwiringPi
- Run from terminal (using sudo to give GPIO access privileges)
sudo ./<executableName>
If that all works fine, then try the following code to control MOTOR-1 on Robot-HAT v4:
- Simple 6v TT-Motor pluged into Motor-1 plug
- This a test program written in C++ and complied with g++
// motor_test4.cpp
// Test program to control motor direction and speed using I2C and GPIO
// Uses wiringPi library for GPIO control
// C++17 standard
#include <iostream> // C++ library for std::cout, std::cerr
#include <wiringPi.h> // for wiringPiSetupGpio, digitalWrite, pinMode
#include <cstdint> // for uint8_t, uint32_t. It is equivalent of C header <stdint.h>
#include <fcntl.h> // for open
#include <unistd.h> // for read, write, close, sleep (see below for chrono and thread alternatives)
#include <linux/i2c-dev.h> // for I2C_SLAVE
#include <sys/ioctl.h> // for ioctl
#include <wiringPi.h> // for wiringPiSetupGpio, digitalWrite, pinMode
#include <csignal> // for signal handling (capture Ctrl-C)
//#include <cstdlib> // for std::exit
#include <chrono> // for std::chrono - unlike sleep() in unistd.h, can explicitly define duration (milliseconds, seconds etc)
#include <thread> // for std::this_thread::sleep_for - unlike sleep() in unistd.h, does not block entire program and frees CPU for other tasks
#include <cmath> // for std::round()
#include <cerrno> // for display error
#include <cstring> // for strings
#include <stdexcept> // for exception
// INITIALIZE CONSTANTS for MOTOR 1 only for simplicity during testing
const char *i2cDevice = "/dev/i2c-1"; // I2C device file path
const uint8_t SLAVE_ADDR = 0x14; // Robot-HAT v4 MCU I2C slave address
const uint8_t MOTOR1_PMW_REG = 0x2D; // PMW13 register for MOTOR1 (initiall had wrong register 0x2C for MOTOR1)
const uint8_t T3_PERIOD_REG = 0x47; // Timer 3 PERIOD register address
const uint8_t T3_PRESCALER_REG = 0x43; // Timer 3 PRESCALER register address
const uint16_t PRESCALER_VALUE = 848; // Value generated from motr class in motor.py and pmw.py modules (848 calc from pmw.py)
const uint16_t PERIOD_VALUE = 849; // Value generated from motr class in motor.py and pmw.py modules (849 calc from pmw.py)
#define MOTOR_1_DIRECTION_PIN 23 // wiringPi pin number for GPIO23
#define BUTTON_PIN 25 // wiringPi pin number for GPIO25
/************ Set Global Flag and handle program termination *****************************/
volatile std::sig_atomic_t stop_program = 0; // global flag to signal program termination
void signal_handler(int signum) {
if(signum == SIGINT) {
std::cout << "Ctrl-C (SIGINT) captured, stopping program..." << std::endl;
stop_program = 1; // set flag to stop program
}
}
/************************** Helper Functions *****************************/
// Function to write values to prescaler, period and PMW registers
// Registers are 1-byte address (i.e. 8 bits)
// Registers are expecting values 2-byte (16-bit) long
// Create a 3-byte buffer: 1 byte for register address + 2 bytes for value to be written split into MSB and LSB
// write 3 bytes to I2C device file
int writeI2CRegister(int file, uint8_t reg, uint16_t value) {
uint8_t buffer[3];
buffer[0] = reg; // register address
buffer[1] = (value >> 8) & 0xFF; // high byte (msb)
buffer[2] = value & 0xFF; // low byte (lsb)
if (write(file, buffer, 3) != 3) {
return -1; // write failed
}
return 0; // write successful
}
// function to set motor direction GPIO pin
int setMotorDrection(int direction) {
// initialize wiringPi which automatically defaults to BCM GPIO numbering
if (wiringPiSetupGpio() == -1) {
std::cerr << "Error setting up wiringPi" << std::endl;
return 1;
}
// Inititalize motor 1 direction pin
pinMode(MOTOR_1_DIRECTION_PIN, OUTPUT);
// digitalWrite(MOTOR_1_DIRECTION_PIN, direction)
if (direction == 1) {
digitalWrite(MOTOR_1_DIRECTION_PIN, HIGH); // Set direction HIGH for FORWARD
std::cout << "Motor 1 direction set to FORWARD" << std::endl;
} else {
digitalWrite(MOTOR_1_DIRECTION_PIN, LOW); // Set direction LOW for REVERSE
std::cout << "Motor 1 direction set to REVERSE" << std::endl;
}
return 0; // return success
}
// function to set motor speed
int motorSpeed(int file, int speed) {
// Calculate PMW value based on speed percentage
if (speed < 0 || speed > 100) {
std::cerr << "Invalid speed percentage. Must be between 0 and 100." << std::endl;
return -1;
}
/* static_cast<destination_type>(source_expression);
performs explicit, compile-time type conversions between related types.
It is considered a safer alternative to the C-style cast because it is more restrictive
and performs type checks at compile time, making code more readable and helping to catch potential errors early. */
uint16_t pmwValue = static_cast<uint16_t>((PERIOD_VALUE * speed) / 100); //
if(writeI2CRegister(file, MOTOR1_PMW_REG, pmwValue) < 0) {
throw std::runtime_error("FAILED to write motor speed to PMW register");
}
return 0; // return success
}
/***************************************MAIN FUNCTION *************************************/
// main function
int main() {
/************************ Open I2C Bus and set slave addr ***************************/
int file = open(i2cDevice, O_RDWR);
if (file < 0) {
std::cerr << "Failed to open the I2C bus" << std::endl;
return 1;
}
if (ioctl(file, I2C_SLAVE, SLAVE_ADDR) < 0 ){
close(file);
std::cerr << "Failed to set I2c slave address: " << std::strerror(errno) << std::endl;
return 1;
}
/****************** Set up PMW timer (Timer 3 assigned to motors) ******************/
// Write period value to timer 3 period reg 0x47
if(writeI2CRegister(file, T3_PERIOD_REG, PERIOD_VALUE) < 0) {
throw std::runtime_error("Failed to write PERIOD to Timer 3"); // returns exception object, control flow if error occurs
std::cerr << "Failed to write PERIOD to Timer 3" << std::endl; // std output stream display error mesage to user
close(file); // close file if fails
return 1;
}
// Write prescaler value to timer 3 prescaler reg 0x43
if(writeI2CRegister(file, T3_PRESCALER_REG, PRESCALER_VALUE) < 0) {
throw std::runtime_error("Failed to write PRESCALER to Timer 3");
std::cerr << "Failed to write PRESCALER to Timer 3" << std::endl;
close(file);
return 1;
}
/************* get user input for directions and motor speeds to test *************/
int direction1 = 0; // 1 for FORWARD, 0 for REVERSE (default to REVERSE)
std::cout << "Enter starting motor direction (1 for FORWARD, 0 for REVERSE): ";
std::cin >> direction1; // assign direction from user input
int speed1 = 0; // speed value from 0 to 100 percent
std::cout << "Enter motor speed percentage (0 to 100): ";
std::cin >> speed1; // assign speed from user input
int direction2 = 0; // 1 for FORWARD, 0 for REVERSE (default to REVERSE)
std::cout << "Enter second motor direction (1 for FORWARD, 0 for REVERSE): ";
std::cin >> direction2;
int speed2 = 0;
std::cout << "Enter second motor speed percentage (0 to 100): ";
std::cin >> speed2;
int direction3 = 0;
std::cout << "Enter third motor direction (1 for FORWARD, 0 for REVERSE): ";
std::cin >> direction3;
int speed3 = 0;
std::cout << "Enter third motor speed percentage (0 to 100): ";
std::cin >> speed3;
/************** run motor using user input and capture SIGINT and close gracefully otherwise motor may continue to run ************/
// Register the signal handler for SIGINT (Ctrl-C) and raise error if this fails
if((std::signal(SIGINT, signal_handler) == SIG_ERR) < 0) {
std::cerr << "Error setting up signal handler" << std::endl;
close(file); // added to close file
return 1;
}
// Otherwise let user know program is running and how to stop it
std::cout << "Motor test running. Press Ctrl-C to stop." << std::endl;
// Initialize GPIO motor direction for motor 1
if (setMotorDrection(direction1) != 0) {
close(file);
return 1;
}
// Main loop to run motor until stop_program flag is set by signal handler
while (! stop_program) {
// Call motorSpeed function to set motor speed
try {
setMotorDrection(direction1);
motorSpeed(file, speed1);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
setMotorDrection(direction2);
motorSpeed(file, speed2);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
setMotorDrection(direction3);
motorSpeed(file, speed3);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
} catch (const std::runtime_error &e) {
std::cerr << "Runtime error: " << e.what() << std::endl;
close(file);
return 1;
} catch (const std::exception &e) {
std::cerr << "Error: " << e.what() << std::endl;
close(file);
return 1;
}
}
std::cout << "Stopping motor and exiting program..." << std::endl;
/*************************** Perform steps to close motor and I2C device gracefully***********************************/
// Set MOTOR1 PMW to 0% speed (0 -> 0x0000) to stop motor
if (writeI2CRegister(file, MOTOR1_PMW_REG, 0) < 0) {
std::cerr << "Failed to write 0 to MOTOR1 PMW register" << std::endl;
}
close(file); // Close the I2C device file
std::cout << "Slave closed." << std::endl;
return 0;
}
Again, cut and paste into your favorite IDE and save file with .cpp extension. Then compile against the WringPi library using the -lwiringPi flag at the end otherwise it will fail to compile.
- Compile this with g++
g++ <yourFile.cpp> -o <executableName> -wiringPi
- Run from terminal using:
sudo ./<executableName>
If it works, it will ask you to input direction (0 or 1 only) three times, and motor speed (0-100 only) three times and then cycle through these until signaled to stop with Ctrl-C
WARNING - Voltage ISSUE Pi5 with Robot-HAT v4
Please be warned! The robot-HAT v4, though compatible with Raspberry Pi 5 cannot supply sufficient amps (3A vs 5A) that the Pi 5 expects. This issue is particularly pronounced when running Ubuntu 24.04 LTS on the Pi 5.
I took some trial and error to find a TT-motor that did not over-draw on the amps and cause the Pi 5 to fail and turn off suddenly. Though this did not damage my Pi 5… it COULD DAMAGE YOUR PI5!
I offer my code and experience to those that might want to try C++ code on the Robot-HAT v4. I give NO guarantee that either the code, or running TT- motors on v4 HAT on Pi5 with this known power issues will not result in a damaged and unusable HAT or Pi5!
ATTEMPT THIS AT YOUR OWN RISK
The newer Robot-HAT 5 (when available) and Fusion HAT+ expansion board should be a better match with a Pi 5 as they claim to supply 5A. I have not tried this yet but will do so when I purchase one of these.
If anyone has one of these, feel free to adapt code above.
Good luck!