I. Design Methods
a. Microcontroller architecture
This system uses two Arduino RP2040 Nano Connect microcontrollers. This microcontroller architecture has I2C support for serial communication and a large variety of digital and analog I/O pins, interrupt pins, and PWM enabled pins sufficient for the closed loop position control of at least two motors simultaneously by one board. The variety of pins available also allows for the implementation of a variety of user inputs. This, combined with the small form factor and pin configuration of this board made it ideal for rapid breadboard prototyping and debugging.
b. Control and estimation strategy
The controller microcontroller (MCU) operates under open-loop control and produces joint references based on user input, predefined sequences, or both. The user may input reference angles manually by turning two 10k Ohm potentiometers. Voltage readings from these potentiometers are converted at a 10-bit resolution by the controller MCU, and are then mapped to a range of [-90°, 90°]. The auto-sweep mode generates a 0.5 Hz square wave between 0° and 90°. The stabilizer mode accepts user input for controlling the end link (J2) and estimates the position of the base joint (J1) needed to stabilize the link, that is, hold the tip of the link at the same point in space. The expression that relates J2 to J1 is detailed in Section III.
The peripheral controller operates under closed-loop control at 100 Hz. The peripheral MCU receives feedback in the form of encoder counts from the integrated encoders at each joint motor. These counts are converted into angular positions in degrees, accounting for the pulses-per-revolution of the encoder and the reduction of the motor gear head. For accurate position control of each joint, we designed two proportional-integral-derivative (PID) controllers for the base (J1) and end link (J2). These controllers were tuned using the Ziegler-Nichols method, with ultimate gains estimated to be Ku,j1 = 100, and Ku,j2 = 50. The base motor was found to have a higher ultimate gain as its working load is the greater of the two motors. Tracking errors at steady state for all joints across the operating modes were found to be within 2°, shown in Section V.
c. Communication protocol and timing
We used the I²C communication protocol for short range connectivity between the two MCUs. The MCUs communicate over serial at a 19200 baud rate and operate with leader-driven timing. The peripheral waits to receive commands from the controller, which sends 9 byte packets at 40 Hz to the peripheral, containing the current command type and reference for each joint. The peripheral does not send status updates (feedback) to the controller, as we designed the controller to operate under open-loop control.
d. Mode logic and behavior implementation
The controller had three modes, Manual, Auto, and Stabilize, which were defined as enumerator variables, integer values with names assigned to them for readability. To switch between modes, the user presses a button, shifting the mode to the next mode in the enum class (Manual → Auto → Stabilize → Auto → …). This way the user may change modes simply and quickly.
The controller MCU has a switch case structure containing the three modes as cases, each with their own type of reference values to send to the peripheral via I²C communication. Manual mode simply reads off the user-controlled potentiometer values and sends those as the reference values for the peripheral’s motors and PID control to follow. Auto mode generates a square wave from 0° to 90° at 0.5 Hz and assigns 0° or 90° as the reference values. Finally, stabilize mode sends the end link’s potentiometer value as its reference value and derives the base link’s reference value needed to approximately maintain the end link’s position using a geometric equation with small angles assumption.
e. Safety, E-stop, and validation approach
The potentiometer mapping of the joint angle J1 is limited to [-90°, 90°] to prevent the J1 motor from overturning and wrapping the end link motor lines around the armature. We tested that the range was properly mapped by outputting the joints’ angles via serial monitor as the full range of the potentiometers were tested during manual mode.
In case of any bugs or out of control commands, we created the E-stop button to shut off all motor activity on the peripheral MCU by sending a command via I2C communication, rendering the robot arm harmless. The E-stop was tested in all modes to ensure proper functionality. After the button was pressed, the joints were unable to move regardless of mode and potentiometer movement. Once the button was pressed a second time, exiting the E-stop state, I2C and motor functionality resumed as normal.
II. System Architecture
Figure 1: System Architecture for the Dual-MCU 2-DoF Robotic Arm
Figure 1 shows how the full system operates from the user inputs all the way to the motion of the 2-DoF arm. The controller microcontroller (MCU) reads the potentiometers and buttons as inputs, handles the mode logic, and sends I²C commands to the peripheral MCU. The peripheral MCU reads the encoder feedback, runs the PID control for both joints, and generates the PWM and direction signals that drive the motor driver. The motor driver controls the DC motors, which move the two joints of the mechanical arm. As the joints rotate, the encoders measure their positions and send that information back to the Peripheral MCU to close the feedback loop.
III. Code Overview & Documentation
The code for the 2-DoF robot arm consists of the controller MCU code and the peripheral MCU code. The controller MCU is responsible for user interface via potentiometers and buttons, trajectory generation, timing, and mode management, while the peripheral MCU is responsible for control, sensing, and logging.
The controller MCU code’s process is as follows:
Initialization of all variable before void Setup()
Void Setup(): initialize pins for potentiometers, buttons, and LEDs, and setup up Wire.being() for I²C communication
Void loop()
Check if the E-Stop button has been pressed
If pressed, send the E-stop command to peripheral, and loop the void loop() section with a return statement until the E-Stop button is pressed again
The second E-stop press will send a command to reset the E-stop state/button and the code will continue normally
Check if the Zeroing Button has been pressed
If pressed, the void loop() section is looped with a return statement, allowing the user to adjust the potentiometer knobs as desired until the zeroing button is pressed again,
The second zeroing button press will send the zeroing joints command to the peripheral, define the current position as zero and adjust the potentiometer values with an offset to match the new reference zero
Check if the mode button has been pressed
There are three modes: MODE_MANUAL, MODE_AUTO_SWEEP, MODE_STABILIZE corresponding to numbers 0, 1, and 2, respectively
The mode always begins in Manual (0), and when the mode button is pressed, the mode will switch from the current mode to the next one (i.e. from Manual → Auto → Stabilize → Manual)
After checking all buttons, the code checks that the gamestate is in PLAYING and not ESTOP_STATE or SETUP, and checks that the enough time has elapsed for the next I²C message send
If enough time has elapsed, the function runModeSample() is called which:
Reads the potentiometers values and depending on which mode the code is in (Manual, Auto, or Stabilize), a switch case is used to send different targets to the peripheral depending on the mode
Manual: send the potentiometer readings as the reference for the peripheral
Auto: references are generated by a square wave which switches from 0° to 90° every second
Stabilize: reference for joint 2 (the end link/fin) is read from the potentiometer, and the reference for joint 1 (the base/cube) is calculated using the equation
J1reference=L2L1J2reference, (1)
where L1 and L2 are the lengths of joint 1 and joint 2. The equation was derived from the system’s geometry with planar and small angles assumptions.
The command CMD_SET_TARGETS and reference/target values are then sent to peripheral containing: cmd (1 byte), target1 (4 bytes), and target 2 (4 bytes)
The follower MCU code’s process is as follows:
Initialization of all variables before void Setup() (motor pins, motor parameters, PID parameters, PID class variables from PID_v1_bc.h library, timing constants, etc.)
Void Setup(): initialize motors pins, encoders pins, interrupt pins, I²C wire address, PID mode and output limits (+-255), and zero encoder counts
The void loop() consists of a switch case structure with cases: CMD_SET_TARGETS, CMD_ESTOP, CMD_ZERO_JOINTS, and CMD_RESET_ESTOP
CMD_SET_TARGETS:
Take the encoders’ counts to determine the current motor positions and input the positions as the input for the PID calculations performed by the pid.Compute() function from the Arduino IDE library
Filter any possible Nans or errors from setpoints (the motors’ targets)
Set the motors outputs with function setMotor() (PWM command of 0-255 in whichever direction the motor is headed)
CMD_ESTOP:
Turns the PID control to Off (MANUAL) using the library’s SetMode() function
Use the SetMotor() function to set motor outputs to zero
CMD_ZERO_JOINTS:
Call function resetEncoders() which sets encoder count, motor target, and PID inputs to zero
CMD_RESET_ESTOP:
Turns the PID control to On (AUTOMATIC) using the library’s SetMode() function
Function receiveEvent() for I²C communication
The I²C communication expects 9 byte packages from the controller (command byte and 2 floats)
Read the command byte into the variable cmd, and the two motor reference/target floats into setpointJ1 and setpointJ2
Interrupt service routines (ISRs)
ISRs are attached to encoder A and B of encoders J1 and J2 (4 ISRs in total)
Each one detects the RISING state of the encoder and determines the direction from the state of encoder A or B
IV. I²C Protocol
The I2C bus communicates from the controller MCU to the peripheral via address 8. The command packets consisted of 9 bytes, where the first byte was the command (0-3), the next four bytes contained the base link’s reference value, and the last four bytes contained the end link’s reference value. All Wire.write() statements are sent every 25 ms (40 Hz) and called between Wire.beginTransmission() and Wire.endTransmission() to buffer the information and ensure that all bytes are sent as one package. To check that our I2C communication is working properly, we print out an error variable which is equal to Wire.endTransmission() (0 = success). In the receiveEvent(), packages are checked to have nine bytes available before reading in the values to ensure that there would be no missing data. The bytes are then converted to floats using the function memcpy() to safely copy data between variables of different types. The follower applies the command to a switch-case every loop (10 ms), and performs the corresponding command behavior: CMD_SET_TARGETS runs the PID control, CMD_ESTOP disables the PID control and stop motors, CMD_ZERO_JOINTS resets encoder counts, and CMD_RESET_ESTOP re-enables the PID control. While there is no clear timeout logic, after sending CMD_ESTOP, no more commands are sent until a second button press sends over CMD_RESET_ESTOP to resume typical functionality.
Bill of Materials: Item No. Component Description Qty 1 Arduino Nano RP2040 Connect Master MCU for open loop user inputs and mode control 1 2 Arduino Nano RP2040 Connect Follower MCU for closed loop PID control and encoders 1 3 N20 DC Motor w/ Hall Effect Encoder, 50:1 Joint 1 motor with quadrature encoder 1 4 N20 DC Motor w/ Hall Effect Encoder, 50:1 Joint 2 motor with quadrature encoder 1 5 L293D Motor Driver IC Dual H-bridge motor driver 1 6 Ceramic Capacitor (1 uF) Decoupling for motor driver 2 7 Potentiometer (10 kΩ) User input for reference angles J1, J2 2 8 Pushbutton Mode, Zero, E-Stop 3 9 LED Indicator Status & mode feedback to user 2 10 Resistor (10 kΩ) Pull-down resistors for buttons 3 11 Resistor (4.7 kΩ) Pull-up resistors for I2C Lines 2 12 Breadboard Circuit element mounting 4 13 Jumper Wire Signal and power wiring 30 14 9V Battery & Header Motor power supply 1 set 15 Micro USB Cable MCU programming & logic power 2 16 3D-Printed 2-DOF Arm Assembly Base motor housing, arm motor housing, and end link 1 set