Humble

README.md

@@ -0,0 +1,147 @@
+
+## Introduction
+
+This repository presents a complete example of how to repurpose an **ANET A8 3D printer motherboard** into a functional actuator system for a differential drive robot, fully integrated with the **ROS 2 control framework** (`ros2_control`). It provides both the **firmware** that runs on the microcontroller and the **hardware interface plugin** required to bridge the physical system with the ROS 2 ecosystem.
+
+This project serves two main purposes:
+
+1. **Hardware Repurposing Reference**: It provides a practical, working example of how inexpensive 3D printer electronics can be reused in robotics applications.
+2. **ROS 2 Control Integration Example**: It offers a minimal but complete implementation of a custom hardware plugin for `ros2_control`, useful as a reference or learning resource.
+
+
+The actuator system is composed of **two stepper motors**, each equipped with an **AS5600 absolute magnetic encoder** to track shaft position. The motors are driven by **A4988 stepper drivers** onboard the ANET A8 motherboard. The board uses an **Atmega1284p microcontroller**, which is compatible with the Arduino toolchain through the [Sanguino board definition](https://github.com/Lauszus/Sanguino), although it is not officially supported by the Arduino IDE.
+
+All communication between the robot and the ROS 2 computer occurs over a **serial connection**, with velocity commands sent to the microcontroller and encoder data streamed back to the host. A **custom `SystemInterface` plugin** for `ros2_control` implements this logic, enabling real-time control and feedback integration.
+
+![Dual Stepper Controller](media/architecture_overview.png)
+**Figure 1:** System architecture overview. Two stepper actuators with magnetic encoders are driven by an ANET A8 3D printer board running custom firmware. The board communicates via serial with a ROS 2 Humble system, which uses ros2_control to manage real-time control and feedback integration.
+
+
+<div style="display: flex; gap: 20px;">
+
+  <div style="flex: 1;">
+    <h3>Software Stack</h3>
+    <table>
+      <tr><th>Component</th><th>Description</th></tr>
+      <tr><td><b>ROS 2 Humble</b></td><td>Target ROS 2 distribution</td></tr>
+      <tr><td><code>ros2_control</code></td><td>Hardware abstraction & real-time control</td></tr>
+      <tr><td><code>diff_drive_controller</code></td><td>Velocity command + odometry</td></tr>
+      <tr><td><code>joint_state_broadcaster</code></td><td>Publishes joint states</td></tr>
+      <tr><td><b>Launch & Config</b></td><td>YAML + launch files</td></tr>
+    </table>
+  </div>
+
+  <div style="flex: 1;">
+    <h3>Repository Contents</h3>
+    <table>
+      <tr><th>Path</th><th>Description</th></tr>
+      <tr><td><code>src/, include/</code></td><td>C++ hardware plugin implementation</td></tr>
+      <tr><td><code>description/urdf/</code></td><td>Xacro robot description</td></tr>
+      <tr><td><code>config/</code></td><td>Controller YAML config</td></tr>
+      <tr><td><code>launch/</code></td><td>System bringup launch files</td></tr>
+      <tr><td><code>firmware/</code></td><td>Atmega1284p firmware source</td></tr>
+    </table>
+  </div>
+
+</div>
+
+
+
+
+The system is tested and functional. It supports closed-loop velocity control, publishes joint states, integrates cleanly with the ROS 2 navigation stack, and can be driven using tools such as `teleop_twist_keyboard` or custom commands. This setup is a solid foundation for building more complex mobile robots or for learning how to write real `ros2_control` hardware interfaces from scratch.
+
+---
+
+## Installing the Hardware Plugin
+
+This section provides instructions for installing and using the custom hardware plugin developed for the `ros2_control` framework. It assumes a working installation of **ROS 2 Humble** and basic familiarity with ROS 2 concepts. Firmware installation for the ANET A8 board is addressed in a later section.
+
+Begin by cloning this repository into the `src/` directory of your ROS 2 workspace:
+
+```bash
+cd ~/ros2_ws/src
+git clone -b humble https://github.com/ricdigi/ros2_dual_stepper_controller.git
+```
+
+Next, ensure that all required system dependencies are installed. The plugin relies on core components of the ROS 2 control stack, including controller management and joint state broadcasting:
+
+```bash
+sudo apt install ros-humble-ros2-control \
+                 ros-humble-ros2-controllers \
+                 ros-humble-diff-drive-controller \
+                 ros-humble-joint-state-broadcaster \
+                 ros-humble-controller-manager \
+                 ros-humble-robot-state-publisher \
+                 ros-humble-xacro
+```
+
+Optional tools for manual control and visualization can be installed with:
+
+```bash
+sudo apt install ros-humble-teleop-twist-keyboard \
+                 ros-humble-rviz2
+```
+
+After installing system packages, use `rosdep` to resolve any remaining ROS-specific dependencies:
+
+```bash
+cd ~/ros2_ws
+rosdep install --from-paths src --ignore-src -r -y
+```
+
+Finally, build the workspace. Ensure your ROS 2 environment is properly sourced before compiling:
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+Once the build completes, source the workspace:
+
+```bash
+source install/setup.bash
+```
+
+You are now ready to launch and test the system.
+
+## Launching the System
+To bring up the dual stepper controller system, use the provided launch files. The main launch file is located in `launch/dual_stepper_controller.launch.py`. This file sets up the necessary nodes and configurations for the hardware interface, controllers, and robot state publisher. Moreover, it launches rviz2 for visualization and debugging.
+
+Run the following command to start the system:
+
+```bash
+ros2 launch ros2_dual_stepper_controller test_hardware.launch.py
+```
+
+Note: the system will run only if the ANET A8 board is connected via USB and the firmware is correctly installed and running (see the Firmware section below).
+
+## Installing the Firmware on the ANET A8 Board
+
+The firmware is written in C++, and integrating some Arduino library syntax, for the **Atmega1284p** microcontroller on the ANET A8 motherboard. It implements a minimal serial protocol to receive velocity commands from the ROS 2 host and return encoder readings from the attached AS5600 sensors.
+
+### Requirements
+
+The firmware is designed to be compiled and uploaded using **PlatformIO**. To install PlatformIO, follow the instructions on the [official website](https://platformio.org/install/cli). A working Python installation is required. Moreover, make sure to install the shell commands.
+
+### Uploading the Firmware
+
+1. Navigate to the `firmware/` directory:
+
+   ```bash
+   cd firmware
+   ```
+
+2. Open the `platformio.ini` file and set the correct USB upload port:
+
+   ```ini
+   upload_port = /dev/ttyUSB0  ; or COM3 on Windows
+   ```
+
+3. Compile and upload the firmware:
+
+   ```bash
+   platformio run --target upload
+   ```
+
+After upload, the microcontroller will automatically reset and start listening for velocity commands over the specified serial port. Ensure that the serial port specified here matches the one configured in your ROS 2 `<ros2_control>` URDF tag.
+

firmware/include/MagneticEncoder.h

@@ -11,10 +11,11 @@ class MagneticEncoder {
     MagneticEncoder();
 
     void encoderInit(uint8_t MUX_A, uint8_t MUX_B, uint8_t DIR_A, uint8_t DIR_B);
-    void readSensors();
+    bool readSensors();
 
     float getEncoderAData() { return enc_A_data; }
     float getEncoderBData() { return enc_B_data; }
+    float getEncoderUpdateFrequency() { return encoderUpdateFrequency; }
 
   private:
 
@@ -33,6 +34,9 @@ class MagneticEncoder {
     uint8_t currentChannel = 1;
     bool waiting = false;
 
+    float encoderUpdateFrequency = 20.0;
+    float lastUpdateTime = 0.0;
+
     void selectMuxChannel(uint8_t channel);
 
 };

firmware/include/SerialComm.h

@@ -36,7 +36,19 @@ class SerialComm {
     float   getSpeedA();            // Extract motor A speed
     float   getSpeedB();            // Extract motor B speed
 
-    void clearCommand();          
+    void clearCommand();
+
+    // Variables relative to the comunnication protocol
+    inline static const uint8_t HEADER = 0xAA;
+
+    inline static const uint8_t VEL_CMD = 0x01;
+    inline static const uint8_t ENC_CMD = 0x02;
+    inline static const uint8_t ENABLE_CMD = 0x03;
+    inline static const uint8_t DISABLE_CMD = 0x04;
+
+    inline static const uint8_t VEL_DATA_LEN = 8;
+    inline static const uint8_t ENC_DATA_LEN = 8;
+    inline static const uint8_t ENABLE_DATA_LEN = 1;
 
   private:
 
@@ -50,15 +62,6 @@ class SerialComm {
 
     State state;
 
-    // Variables relative to the comunnication protocol
-    static const uint8_t HEADER = 0xAA;
-
-    static const uint8_t VEL_CMD = 0x01;
-    static const uint8_t ENC_CMD = 0x02;
-
-    static const uint8_t VEL_DATA_LEN = 8;
-    static const uint8_t ENC_DATA_LEN = 8;
-
     uint8_t cmd;
     uint8_t len;
     uint8_t data[VEL_DATA_LEN];

firmware/include/StepperMotor.h

@@ -17,9 +17,15 @@ class StepperMotor {
   void setSpeedRad(float radPerSec);
   void setAccelerationRad(float radPerSec2);
 
+  void setRotationDir(bool spinDir_) {spinDir = spinDir_; stepper.setPinsInverted(spinDir, false, false);}
+
 private:
   AccelStepper stepper;
 
+  // Set stepper direction CW or CCW as viewd fromm the motor shaft
+  // CW -> spinDir= true, CCW -> spinDir = false
+  bool spinDir = false;
+
   // Motor Pins
   const uint8_t stepPin;
   const uint8_t dirPin;

firmware/platformio.ini

@@ -22,4 +22,4 @@ lib_deps =
 # Added Options specific to our microcontroller
 # Modify the upload_port according to your system
 upload_speed = 57600
-upload_port = /dev/cu.usbserial-2140
+upload_port = /dev/cu.usbserial-1220

firmware/src/MagneticEncoder.cpp

@@ -15,57 +15,66 @@ void MagneticEncoder::encoderInit(uint8_t MUX_A_, uint8_t MUX_B_, uint8_t DIR_A,
   pinMode(DIR_B, OUTPUT);
 
   digitalWrite(DIR_A, LOW);
-  digitalWrite(DIR_B, LOW);
+  digitalWrite(DIR_B, HIGH);
+
+  delay(200);  // Wait for AS5600 to fully power up and stabilize
 
   // Calibrate the encoders in zero position at the start
-  selectMuxChannel(2);
+  selectMuxChannel(1);
   delayMicroseconds(50);
   as5600.begin();  
-  as5600.setDirection(AS5600_CLOCK_WISE);
+  as5600.setDirection(AS5600_COUNTERCLOCK_WISE);
   offset_A = as5600.rawAngle() * AS5600_RAW_TO_DEGREES;
-  Serial.println("First as5600 initialized");
 
-  selectMuxChannel(1);
+  selectMuxChannel(2);
   delayMicroseconds(50);
   offset_B = as5600.rawAngle() * AS5600_RAW_TO_DEGREES;
-  Serial.println("Second as5600 initialized");
 
   // Leave first channel selected
   selectMuxChannel(1);
   delayMicroseconds(50);
 
+  // Successfully initialized
+  Serial.println("Magnetic Encoder initialized successfully.");
+
 }
 
-void MagneticEncoder::readSensors() {
+bool MagneticEncoder::readSensors() {
+  bool bothRead = false;
 
-  if (!waiting) {
+  if ((millis() - lastUpdateTime) >= (1000.0 / encoderUpdateFrequency)) {
 
-    current_reading = as5600.rawAngle() * AS5600_RAW_TO_DEGREES;
+    if (!waiting) {
+      current_reading = as5600.rawAngle() * AS5600_RAW_TO_DEGREES;
 
-    if (currentChannel == 1) {
-      enc_A_data = current_reading - offset_A;
-      if (enc_A_data < 0.0) { enc_A_data += 360.0; }
-      currentChannel = 2;
-    } 
-
-    else { 
-      enc_B_data = current_reading - offset_B;
-      if (enc_B_data < 0.0) { enc_B_data += 360.0; }
-      currentChannel = 1; 
-    }
+      if (currentChannel == 1) {
+        enc_A_data = current_reading - offset_A;
+        if (enc_A_data < 0.0) enc_A_data += 360.0;
+        currentChannel = 2;
+      }
 
-    selectMuxChannel(currentChannel);
-    waiting = true;
-    muxSwitchTime = micros();
+      else {
+        enc_B_data = current_reading - offset_B;
+        if (enc_B_data < 0.0) enc_B_data += 360.0;
+        currentChannel = 1;
+        lastUpdateTime = millis();
+        bothRead = true;
+      }
 
-  }
+      selectMuxChannel(currentChannel);
+      waiting = true;
+      muxSwitchTime = micros();
+    }
 
-  if ((micros() - muxSwitchTime) >= 50 ) {
-    waiting = false;
+    if ((micros() - muxSwitchTime) >= 50) {
+      waiting = false;
+    }
   }
 
+  return bothRead;
 }
 
+
 void MagneticEncoder::selectMuxChannel(uint8_t channel) {
   digitalWrite(MUX_A, channel & 0x01);
   digitalWrite(MUX_B, (channel >> 1) & 0x01);

firmware/src/SerialComm.cpp

@@ -71,6 +71,10 @@ void SerialComm::sendEncoderData(float enc_a_data, float enc_b_data) {
   memcpy(payload, &enc_a_data, 4);
   memcpy(payload + 4, &enc_b_data, 4);
 
+  if (Serial.availableForWrite() < 15) {
+    return;
+  }
+
   Serial.write(HEADER);
   Serial.write(ENC_CMD);
   Serial.write(ENC_DATA_LEN);
@@ -87,6 +91,10 @@ bool SerialComm::hasCommand() {
   return commandReady;
 }
 
+uint8_t SerialComm::getCommand() {
+  return cmd;
+}
+
 float SerialComm::getSpeedA() {
   float val;
   memcpy(&val, data, 4);

firmware/src/StepperMotor.cpp

@@ -11,7 +11,7 @@ StepperMotor::StepperMotor(uint8_t stepPin_, uint8_t dirPin_, uint8_t enablePin_
   }
 
 void StepperMotor::computeConversionFactor(){
-  radToMicrosteps = stepsPerRev * microstepping / (2 * PI);
+  radToMicrosteps = float(stepsPerRev * microstepping) / (2.0f * PI);
 }
 
 void StepperMotor::setUpEnablePin(){
@@ -31,7 +31,8 @@ void StepperMotor::setAccelerationRad(float radPerSec2){
 void StepperMotor::setSpeedRad(float radPerSec){
   targetSpeedRad = radPerSec;
   maxSpeedMicrosteps = toMicrostep(targetSpeedRad);
-  stepper.setMaxSpeed(maxSpeedMicrosteps);
+  stepper.setMaxSpeed(fabs(maxSpeedMicrosteps));
+  stepper.setSpeed(maxSpeedMicrosteps);
 }
 
 void StepperMotor::enable(){
@@ -43,8 +44,7 @@ void StepperMotor::disable(){
 }
 
 void StepperMotor::run(){
-  stepper.moveTo(stepper.currentPosition() + 1e6);
-  stepper.run();
+  stepper.runSpeed();
 }
 
 void StepperMotor::stop(){