This repository contains software, firmware and hardware to operate a commercial cartpole robot. It provides integration with Neural Control Tools. The cartpole robot producer and developers of this repository are independent parties hence we cannot guarantee the compatibility with future versions of the robot.
-
USB interface to a PC
-
Support for original STM but also for Zybo Z7-20
-
Integration with tailored Cartpole Simulator and tools for system identification with neural networks and control.
-
Various control modes
- Control from PC: PID, MPC and neural controller
- Control from STM: PID
- Control from Zybo Z7-20: PID and FPGA-accelerated neural controller
All controllers can stabilize the pole in the upright position and follow target position. MPC and neural controller can also swing-up the pole.
-
Template with examples for programing experiment sequence for automated data collection.
-
Auxiliary functions allowing easier calibration of the robot, motor safety cut off, and more.
To operate cartpole, beyond the cartpole robot itself (see "Original setup" below), you need:
- If you want to use it with STM32:
- Make sure to buy cartpole with STM32 board.
- STM32 programmer (e.g. STLink or J-Link)
- If you want to use it with Zybo-Z7-20:
-
We are using Zybo-Z7 -20 board, and our current designs consumes over the half of the FPGA resources. With some additional optimization it should be possible to fit well performing neural controllers on even smaller Zynq chips. No matter if you switch to smaller or bigger board, you will probably need to adjust the design regarding input and output pins and buttons, LEDs and switches assignments.
-
You need to prepare the analog filter with voltage divider for angle (ADC) input and H-bridge for motor. See below for the details, here just list of items:
- 12V, 5A power supply for the motor (usually comes with the cartpole robot)
- 2 x Pmod TPH2 (12-pin Test Point Header), we use it to solder H-bridge and analog filter on it
- H-Bridge: Pololu TB6612FNG Dualer Motortreiber
- 1 x Ferrite bead, we took just what we had in house to clean power supply for potentiometer
- Capacitors:
- 1 x 1uF for filtering angle signal from potentiometer (RC time constant = 0.1 ms; you can adjust this one)
- 1 x 20uF and 1 x 4.7 uF for filtering power supply for potentiometer
- 1 x 470uF for filtering power supply for H-Bridge - probably facultative, just any big capacitor will do
- Resistors:
- 12 kOhm for voltage divider for potentiometer
- 100 Ohm for filtering angle signal from potentiometer (RC time constant = 0.1 ms)
- Some wires to solder connections
- Barrel connector for power supply to be soldered to H-Bridge
-
- Additionally, you might want to buy:
- Possibly a cable to connect the STM32 or Zybo board (micro USB) to PC, not sure if included with cartpole robot or Zybo. You probably want a rather long one for convenience (1-2m).
- Spare motor with Encoder - we use Pololu 19:1 Metal Gearmotor, 37Dx68Lmm, 12V with 64 CPR Encoder #4751
- Power supply for Zybo-Z7 board - 5V, 2.5A, barrel connector; the board can be powered also from USB.
- SD card and SD card reader - to flash Zybo board from it without need to connect to computer after start up (should be also possible to program the chip with flash memory on Zybo board, but we did not test it)
- Spare STM boards if you want to work mostly with them - they seem to be fragile
- metal bars for experiments with poles of different mass and length - we had these in-house so you have to find on your own where to buy them. With a rough measurement the pole mounting hole is 6mm in diameter, the pole is 5.7mm in diameter.
- Some material to decorate the cartpole - don't forget the artistic side of the project! ;-)
We bought our cartpole robot on AliExpress.
As of today there are a few links to the product e.g. this.
It is worth to search for "inverted pendulum" on AliExpress to find the cheapest option and compare with the picture below.
Out of different versions we have the one using STM32 (ST32F103C8T6) board. Not the one with Arduino.
If you plan to use Zybo-Z7-20 instead, you might be fine buying just the mechanical part of the robot
(but probably need to buy power supply for the motor separately in such a case).
.
Unfortunately the Digilent h-bridge PMODs does not meet specification of the cartpole motor.
The original setup with STM32 uses TB6612FNG H-brdige.
We bought it as Pololu TB6612FNG Dualer Motortreiber
and build our own PMOD H-Bridge by soldering it to Pmod TPH2.
Below hopefully self-explanatory picture on how to prepare the h-bridge.
The PMOD should be connected to the Zybo's JE PMOD connector, the other end should be connected to the motor.
The colors of the wires on the picture match the colors of the wires at the Pololu motor connector.
The barrel connector is for motor supply (12V, 5A).
To get a clean angle measurement from the potentiometer, we designed an analog filter.
We also need a voltage divider as the board delivers 3.3V to the potentiometer
and ADC (XADC) on Zynq operate in the range 0-1V.
The potentiometer output is read from PIN 3 of PMOD connector JA on Zybo Z7-20.
The schematic is provided below:
Below some picture to facilitate the assembly.
Visible capacitor is 1uF for filtering angle signal from potentiometer.
It is place so that it can be easily replaced with different value to adjust the filter's time constant.
Also visible a lot of glue to keep the connection stable when the cartpole is moving.
From top to bottom visible is the ferrite bead,
the 20 uF capacitor filtering potentiometer input
and connection shorting pin 9 to ground (the last one just because haw we placed the circuits).
The remaining resistors and capacitor are hidden under heat shrink tube.
FactoryFirmwareImage.bin and FactoryFirmware CubeIDE contains the fimrware image as shipped by the seller.
It was pulled directly from the STM using a debugger.
It was done long time ago so it might not represent the latest version of the firmware shipped by the seller.
We were able to reload it and operate cartpole with it.
Jerome Jeanine found a way to program the Aliexpress ST micro using open source tools. See his thesis MPPI on a physical cartpole.
- KEIL MDK development environment.
- STLink debugger.
Preferable way to install python packages:
pip install -r requirements.txt in a conda env or pip venv.
The main module is control.py.
Parameters are distributed in globals.py and config_CPP.yml.
The firmware folder has the ST firmware.
The pendulum microcontroller firmware has two modes of operation:
(1) Self-Contained Mode
In this mode, the pendulum is controlled directly by the firmware using an onboard basic PD control scheme. A PC is not required at all for this mode. However you can use the PC to adjust the control parameters on-the-fly, as well as read out set points and other useful values generated by the on-board control algorithm in real time. If you want 'factory mode', just boot up the controller and set it running.
(2) PC Control Mode
In this mode, the pendulum firmware runs as an interface to the physical hardware. Control is performed over USB by an algorithm running on another connected device (PC, FPGA, etc). The firmware outputs the current pendulum angle and cart position at regular intervals, and accepts motor speed and direction as input. The frequency of the control loop running on the PC is governed by the period set for outputting angle/position from the pendulum micro (currently hardcoded to 5 ms).
In either mode, when enabling control of the pendulum for the first time, the firmware will automatically run a calibration routine. During this routine, the cart will slowly move from left to right in order to determine the maximum limits of movement.
Two buttons are used by the firmware
S1
This is the CPU's hard reset.
S2
Used to switch between the 2 modes of operation in the pendulum firmware.
There is a blue LED (L2) that flashes periodically, fast or slow depending on which mode the firmware is operating in. Fast (200 ms period) -> Self-contained mode. Slow (1 s period) -> PC control mode.
The interface to the pendulum firmware is through the pendulum.py module, which provides a series of high-level functions to configure and command various things on the microcontroller. control.py contains an example implementation of a PD controller running the PC and interfacing directly with the pendulum firmware.
Parameters that can be adjusted via the PC are listed below. Note that raw values are used for set points and control gains.
- Angle set point (~3110 is vertical).
- Angle average length (number of samples to average over when determining current angle).
- Angle smoothing factor (0 to 1.0 - used in 1st order low-pass filter).
- Angle control gains (P and D).
- Position set point (0: centre, <0: left, >0: right).
- Position control period (as a multiple of Angle control period - 5 ms).
- Position smoothing factor (0 to 1.0 - used in 1st order low-pass filter).
- Position control gains (P and D).
If enabled, the following data is streamed to the PC at regular 5 ms intervals:
- Current pendulum angle.
- Current cart position.
- Motor speed command calculated by onboard controller (0 when running in PC-control mode).
The zynq folder has the Vivado and Vitis projects for both Zybo and Zedboard boards. The Xilinx tools version is 2021.2. To replicate the projects you need to install Vivado (including Vitis) following the installation guides Vivado
Open Vivado tool and click on the TCL console tab. This tab is place at window bottom. Navigate to zynq folder:
cd path/to/zynq/folderThen run the tcl script using the next command:
source ./zybo.tclThe vivado project will be create and it will be ready for synthesis. Yo can modify this project according your needs.
Vitis projects are compressed in zybo_vitis_pot_motor_test.zip file. So to import then into Vitis follow the next steps:
- Open Vitis using a new workspace
- Click on File -> Import...
- Select "Vitis project exported zip file" option
- Browse zybo_vitis_pot_motor_test.zip file
- Select all the projects
- Click Finish
The projects are ready to be run in the Zybo board.
sys.stdin.read(1) which is used for keyboard input causes the debugger of Pycharm to hang.
For USB-UART bridge if you use FTDI chip - not the case for original stm board, but a default bridge for Zybo board or PMODs you will get 16ms latency due to FTDI chip waiting for minimal number of bytes to transfer to PC. You can disable this feature programmatically.