This project was based off of the Cubli Project from ETH Zurich. The Cubli is a cube that has three reaction wheels to control the balance of the x, y, and z planes of the cube. This enables the cube to balance itself either on an edge or on a corner. A video of the Cubli final design can be found here.

This project aimed to create just a single dimension of the cubli project. We used a Xilinx Zynq board that had a CPU and an FPGA on the same silicon die which enabled us to build some custom architecture on the FPGA to pair with the CPU. We also used Free RTOS to run the program on the CPU side. The FPGA architecture used the free Vivado Webpack for development.

The project consists of digital controls with state space system model representation and LQR optimal controls. The Free RTOS side was used to read in user input, as well as our I2C IMU and run the controls code. The desired motor speed was then fed to the FPGA, which handled creating the necessary PWM waveform, as well as reading our optical encoder.

Step 1: Components Shopping List

Electronics

This project used the Xilinx Zybo Zynq 7000 development board which contains dual-core ARM Cortex-A9 processors as well as a Xilinx 7-series FPGA.
An IMU (MPU-6050) was used to measure the angular speed and position of the pendulum body.
A brushless DC motor was used to drive the freewheel
A standard ESC was used to control the motor
- We used a standard quadcopter ESC, but this is not recommended. We were unable to control the speed of the motor to a fine enough granularity. This is one of the reasons why this project was unsuccessful.
a homemade optical encoder was built to measure the speed of the wheel

Framework

Using Aluminum, a wheel was machined out with as much weight towards the outside as possible. This creates a larger moment of inertia from the spinning wheel which applies a larger amount of torque to the pendulum body.
- If at all possible, get a wheel with a ring of mass on the outside and with spokes inside. This will result in the highest moment of inertia.
A wooden square mount was built with plywood to mount the motor, wheel, IMU, and optical encoder. This will be referred to as the pendulum body in later steps. This is shown in the above pictures.
A mount for the pendulum body was made using two small plates with bearings mounted within them. An aluminum rod was then mounted through the bottom corner of the pendulum body and then through the bearings.
- This method resulted in a fair amount of lateral movement. Try using something more similar to this.

Finding the motor torque constant

The motor torque constant is usually given on the motor data sheet but if not, like in our case, you can use the KV (rpm/v) rating if the motor to find the motor torque constant.

motor torque = 60 / ( 2 * pi * KV )

Optical Encoder

The optical encoder was built using an infrared line sensor that was pointed to the outside of the wheel. Eight black electric tape lines were taped at even intervals on the outside of the wheel. The encoder would detect and output a high value whenever it was above black tape and a low value when it was not. This can be sensed by the Zynq chip and transformed into a rotation speed value.

Step 2: Sensor Inputs

Two different sensors were used in this project, a 6-axis IMU and an optical speed encoder. The IMU used was the MPU-6050 and communicated over an I2C bus to the Zynq board.

The optical encoder was created by placing black tape in stripes around the outside of the flywheel. Then a line following sensor was used to detect the presence or absence of black tape. This signal needed to be amplified and required a custom amplifier board. The schematic for this board is shown in this section. The amplified output was then fed to the FPGA side of the Zynq board.

Step 3: FPGA Fabric

The codebase was split between the FPGA fabric and the ARM Cortex-A9 CPU on the Zynq chip. The FPGA fabric was in charge of outputting a PWM wave to the motor, as well as filtering and reading the motor speed.

Motor Driver Control

The motor driver used determined the output speed by reading in the duty cycle of a 500 Hz PWM waveform from the Zynq board. This PWM generation was done in hardware. This block would take in a duty cycle value between 0-100 and output the PWM to the motor controller. The net effect of this was that all the CPU had to do to set the motor speed was write a value between 1 and 100 to a register. The motor would then spin to a predictable speed.

Glitch Filter

While our optical encoding method for speed detection worked well, the input was noisy at times. The solution to this noise was a simple glitch filter. The block read in from the optical encoder, and if the last 5 readings were high, would output a high value. Otherwise it would output a low value. This debounced the noisy signal from the optical encoder and made it suitable for measurement.

Motor Encoder

This block took in the filtered encoder input and outputted a speed value to the CPU. It worked by counting the clock ticks of a 100 MHz clock between every rising edge of the encoder input. By taking an average of 16 of these values, we were able to get a speed in ticks/piece of tape. Then, knowing that there were 8 pieces of tape on the flywheel, we were able to turn this value into ticks/revolution. This is the value that was finally passed to the CPU. The important thing about this value is that as rotational speed increases, the "speed" value decreases. Although we would have preferred to pass a speed value in rotations/second, we would need to take an inverse, which is very costly in an FPGA.

Step 4: Free RTOS

In order to run all the tasks necessary in realtime, FreeRTOS was used. FreeRTOS is a Real Time Operating System, which means that it will respond to events within a predictable amount of time. This is important as it allows the system to be predictable. The code was designed to have four tasks which follow:

Reading from UART
Reading from ESP8266 WiFi Module
Reading the MPU-6050 IMU
Running a command line console

Reading from UART

This was a straightforward task that polled the UART for new characters. When a new character was found, the character was then placed into a queue, which was read by the command line console task. This could have been implemented using UART interrupts, but polling was simple, worked, and by making this task the lowest priority, it only was allowed to run when there was nothing else to do. This means that it was not wasting CPU cycles polling the UART when other things could be done. The only other concern with polling the UART was if characters were coming in too quickly. In our setup, we polled the UART for new characters every 10 ms, which is significantly faster than any human could type. This may be problematic if the UART console were to be run from a computer script, but we decided the simplicity of the module outweighed that feature.

Reading from ESP8266 WiFi Module

As part of this project we also wanted wireless communication. After all, it would be unfortunate if the cube needed to be connected to a computer. This was accomplished using an ESP8266, a small, easy to use WiFi module available from many different suppliers. The ESP8266 communicates to the Zynq board also using a UART interface, which made it necessary to write another UART driver. For this UART driver, we need to read in many characters at once, which meant that interrupts were required. The interrupt handler wass called whenever a character was read by the UART and adds that data to a queue. The queue is then read by the ESP8266 task, which decides what to do with the character.

The task was composed of a large state machine for initializing the module and reading incoming data. When the task received packets from the module it sent them to the same queue that was used by the UART module, which was read by the console task. This allows the same console commands to be used by both an operator physically connected to the system or over WiFi. This method also allows for code reuse and keeps the code base more organized.

Reading the MPU-6050 IMU

The IMU-6050 is a 6-axis IMU composed of a 3-axis accelerometer and a 3-axis gyroscope. This allows for detection of the angle and rotational speed of the system, which are both important to the controls algorithm. Getting data off the sensor was straightforward. The sensor communicates over I2C with an address of 0x68. Before reading the accelerometer and gyroscope data, the sensor must be taken out of sleep mode by writing 0 to address 0x6B, the power management register. From there, the raw accelerometer and gyroscope readings can be read starting at address 0x3B. Refer to the data sheet and register map for complete functionality.

When we first started playing with the sensor, we noticed that the raw data was not precise as well as being noisy. We used two separate techniques to solve these problems. After looking at the data, we noticed that the IMU appears to have an offset from the correct readings. To solve this problem, we placed the IMU on a level table, where we knew the orientation of each of the accelerometer axis. Then we took and averaged 5 measurements from the IMU. We then took the known orientation and subtracted it from the acceleration it gave us. The result was an offset vector which was subtracted from every IMU reading. This increased the precision of the system significantly. To solve the noise problem we used a simple 5 point averaging filter. Instead of returning every IMU reading, we would store 5 readings and take the average of each value. This helped eliminate a fair bit of the noise we were seeing from the sensor.

From here we were hoping to send this information to the FPGA, which would run the controls algorithm. Unfortunately, we were having difficulties implementing the controls algorithm in Verilog and decided to implement it in C. Because of this, once a valid IMU reading was taken, the RTOS task would then compute the necessary motor speed and send it to the motor through the PWM generation block. While this made the implementation simpler, it would have been nice to run the controls inside of the FPGA which we believe would have improved the control loop.

Reading a Command Line Console

The command line console was responsible for sending higher level commands and polling information, such as the IMU readings from the system. The command line console ran as its own task and takes in data from both the UART and the ESP8266. This allowed the console to be operated directly from a computer or over a wireless network. The task blocked on a FreeRTOS queue until data was available. The task then operated like a state machine, reading in a command name and data. If the command was valid then it would run the associated code and wait for another command. This allowed for flexible commands and was helpful for debugging and commanding the system.

source.zip
Download

Step 5: System Dynamics

Dynamics

The Cubli project works as an inverted pendulum, the unique aspect of the project is how the pendulum is controlled. Usually a motor will control an inverted pendulum directly, with pendulum axis of rotation mounted directly to the motor axis with a 1:K ratio. With this project, the pendulum position is controlled with the free wheel attached to the pendulum arm. Due to conservation of momentum, when the wheel is accelerated in one direction, torque is applied to the pendulum arm in the opposite direction. This results in a second order system, the pendulum, controlled by another second order system, the reaction wheel. This is called a coupled second order control system.

The dynamics transfer equations used can be found in the IEEE paper published on the Cubli Project here.

Step 6: Controls Implementation

System Model: state-space

The standard method of controls, PID, is a little difficult to implement with this system because it is a coupled second order system. This means that the inverted pendulum (second order), is controlled by another second order system (reaction wheel). Usually an inverted pendulum is controlled with a motor directly controlling the angle of the pendulum (i.e. pendulum attached to the motor shaft) which is a 1:1 relationship. PID is generally used for simple, second order or less systems, beyond that, the tuning of a PID system becomes more difficult and too much of a hassle. State-space is easier to plug into more modern and complex control methods like LQR.

Control Method: LQR

Once the system has been modeled in state-space, Matlab can be used to find the K matrix for LQR control of the system. A deep understanding of LQR controls is not necessary if you have access to Matlab, it does all the work for you. But if you don't have Matlab, to find your K matrix you simply plug your state-space model into the algebraic Riccati equation. For this system, you can set the R matrix to all zeros because we are not using any feed-forward signals. The Q matrix will weight the control effort for each state, it is a diagonal matrix and the values of each element can be viewed as a ratio between how important each state of the system is. For this system, the angle of the pendulum body is most important so that should have the highest value in the Q matrix.

Matlab tutorial on state-space and LQR

Step 7: Conclusion

Although we were unable to achieve balance with this system, we were able to learn a lot about the inner workings of SoC systems, as well as controlling these systems using RTOS's. We also learned about some of the pitfalls of designing mechanical systems. We would like to revisit this project at some point, once we look more deeply into topics in machining, brushless motors, and brushless motor control.

Some suggestions for repeating this project:

Bidirectional motor control is crucial, a brushless dc motor is not as important. We found that almost all brushless dc motors are unidirectional because of the large presence of the quad-copter industry. We would suggest to find a motor regardless of brush/less (or even geared/not geared), that has some kind of speed encoder (hall effect or optical) and that can also be controlled quickly and in both directions.